a riemannian-geometry approach for modeling and control of...

Hindawi Publishing CorporationJournal of RoboticsVolume 2009, Article ID 892801, 16 pagesdoi:10.1155/2009/892801

Research Article

A Riemannian-Geometry Approach for Modeling and Controlof Dynamics of Object Manipulation under Constraints

Suguru Arimoto,1, 2 Morio Yoshida,2 Masahiro Sekimoto,1 and Kenji Tahara3

1 Research Organization of Science and Engineering, Ritsumeikan University, Kusatsu, Shiga 525 8577, Japan2 RIKEN-TRI Collaboration Center for Human-Interactive Robot Research, Nagoya, Aichi 463 0003, Japan3 Organization for the Promotion of Advanced Research, Kyushu University, Fukuoka 819 0395, Japan

Correspondence should be addressed to Suguru Arimoto, [email protected]

Received 27 October 2008; Accepted 27 January 2009

Recommended by Heinz Worn

A Riemannian-geometry approach for modeling and control of dynamics of object manipulation under holonomic or non-holonomic constraints is presented. First, position/force hybrid control of an endeffector of a multijoint redundant (ornonredundant) robot under a holonomic constraint is reinterpreted in terms of “submersion” in Riemannian geometry. A forcecontrol signal constructed in the image space of the constraint gradient is regarded as a lifting (or pressing) in the directionorthogonal to the kernel space. By means of the Riemannian distance on the constraint submanifold, stability of position controlunder holonomic constraints is discussed. Second, modeling and control of two-dimensional object grasping by a pair of multijointrobot fingers are challenged, when the object is of arbitrary shape. It is shown that rolling contact constraints induce the Eulerequation of motion, in which constraint forces appear as wrench vectors affecting the object. The Riemannian metric is introducedon a constraint submanifold characterized with arclength parameters. An explicit form of the quotient dynamics is expressed in thekernel space with accompaniment of a pair of first-order differential equations concerning the arclength parameters. An extensionof Dirichlet-Lagrange’s stability theorem to redundant systems under constraints is suggested by introducing a Morse-Lyapunovfunction.

Copyright © 2009 Suguru Arimoto et al. This is an open access article distributed under the Creative Commons AttributionLicense, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properlycited.

1. Introduction

Among roboticsists, it is implicitly known that robot motionscan be interpreted in terms of orbits on a high-dimensionaltorus or trajectories in an n-dimensional configurationspace. Planning of robot motions has been investigatedtraditionally on the basis of kinematics on a configurationspace as an n-dim numerical space Rn [1].

This paper first emphasizes a mathematical observationthat, given a robot as a multibody mechanism with n degreesof freedom whose endpoint is free, the set of all its posturescan be regarded as a Riemannian manifold (M, g) associatedwith the Riemannian metric g that constitutes the robotinertia matrix. A geodesic connecting any two postures cancorrespond to an orbit expressed on a local coordinate chartand generated by a solution to the Euler-Lagrange equationof robot motion that originates only from the force of inertia[2, 3]. It should be emphasized that once the Riemannian

manifold is given corresponding to the n degrees of freedomrobot, the collection of all the geodesic paths describes the“law of inertia” for the manifold. It is also important tonote that geodesic paths are invariant under any choice oflocal coordinates. This Riemannian geometry viewpoint isextended in this paper to an important class of multibodydynamics physically interacting with an object or with envi-ronment through holonomic or/and nonholonomic (butPfaffian) constraints. Holonomic constraints are defined asa set of infinitely differentiable functions from a productmanifold of multibody Riemannian manifolds onto an openset of a 2- or 3-dimensional Euclidean space called thetask space. Such a mapping can be treated as a submersionfrom the product Riemannian manifold to m(= 2- or 3-)dimensional Euclidean space. Hence, holonomic constraintsinduce a Riemannian submanifold with a naturally inducedmetric. An Euler-Lagrange equation is formulated in animplicit function form under such constraints. It is also

2 Journal of Robotics

shown that if the gravity term can be explicitly compensatedand there arises no viscous friction then the geodesicmotion is invariant, that is, it is governed by the “law ofinertia,” under any adequate lifting (or pressing) through thejoint torque injection in the direction along the constraintgradient. An explicit form of the Euler equation whosesolution corresponds to a geodesic on the submanifold isgiven also as a quotient dynamics corresponding to thekernel space as an orthogonal compliment to the imagespace spanned from all the constraint gradients. Based uponthese observations, the well-known methodology of hybrid(position/force) control for a robot whose end effector isconstrained on a surface is re-examined and shown to beeffective even if the robot is of redundancy in its degrees offreedom.

In a latter part of the paper, modeling of dynamicsof grasping and manipulation of a two-dimensional rigidobject with arbitrary shape by using a pair of multijointrobot fingers with spherical finger ends is challenged. It isshown that rolling contact constraints between finger endsand the object surfaces induce not only two holonomicconstraints but also two nonholonomic constraints thatrestrict tangent vectors on the original Riemannian manifoldthat is a product of three manifolds expressing a set ofwhole postures of the two fingers and the object. AnEuler-Lagrange equation for expressing the dynamics ofsuch physical interaction is derived through applying thevariational principle together with deriving a set of the first-order differential equations expressing the contact positionsof the object with both the finger ends. The Riemanniandistance is introduced on the kernel space as an orthogonalcompliment to the image space of all the gradients vectors ofboth contact and rolling constraints. In other words, rollingconstraints are expressed in terms of the first fundamentalforms of given contours of the object and restrict only thetangent vector fields at both the contact points. An explicitEuler-Lagrange equation corresponding to a path on theconstraint submanifold is derived together with a set of thefirst-order differential equations expressed in terms of thesecond fundamental forms of the object contours. Thus, itis shown that rolling constraints can be characterized bymeans of arc length parameters of the object contours thatexpress locations of the contact points and in the sequelare integrable in the sense of Frobenius. A coordinatedcontrol signal called “blind grasping” without referringto the object kinematics or external sensing is proposedand shown to be effective in realizing stable grasping inthe sense of stability on a submanifold. A sketch of theconvergence proof is given on the basis of an extension ofthe Dirichlet-Lagrange theorem to a system of degrees offreedom redundancy by finding a Morse-Lyapunov func-tion and using its physical properties and mathematicalmeanings.

2. Riemannian Manifold: A Set of All Postures

Lagrange’s equation of motion of a multijoint system with 2degrees of freedom (DOF) shown in Figure 1 is described by

z

x

y

z1

O1

P

ω2

J1q1 (or θ1),ω1

d

q2

(or θ2)

O(J0)

x1 x2

Figure 1: A 2-DOF part of vertically revolute robot PUMA-560.

the formula

H(q)q +{

12H(q) + S(q, q)

}q + g(q) = u, (1)

where q = (q1, q2)T denotes the vector of joint angles,H(q) denotes the inertia matrix, S(q, q)q the gyroscopicforce term including centrifugal and Coriolis forces, g(q)the gradient vector of a potential function P(q) due to thegravity with respect to q, and u the joint torque generatedby joint actuators [4]. It is well known that the inertiamatrix H(q) is symmetric and positive definite, and thereexist a positive constant hm together with a positive definiteconstant diagonal matrix H0 such that

hmH0 ≤ H(q) ≤ H0 (2)

for any q. It should be also noted that S(q, q) is skewsymmetric and linear and homogeneous in q. More in detail,the i jth entry of S(q, q) denoted by si j can be described in theform [3]

si j = 12

{∂

∂qj

( n∑k=1

qkhik

)− ∂

∂qi

( n∑k=1

qkhjk

)}, (3)

where H(q) = (hi j(q)), from which it follows apparently thatsi j = −s ji. Since we assume that the objective system to becontrolled is a series of rigid links serially connected througheach rotational joint with single DOF, every entry of H(q) isa constant or a sinusoidal function of components of jointangle vector q. That is, each element of H(q) and g(q) isdifferentiable of class C∞ (infinitely differentiable in q).

When two joint angles θ1 and θ2 are given in θi ∈(−π,π], i = 1, 2, for the 2 DOF robot arm shown in Figure 1,the posture p(θ1, θ2) is determined naturally. Denote the setof all such possible postures by M and introduce a familyof subsets of M such that, for any p ∈ M with joint angles

Journal of Robotics 3

−π π

−π

π

q1

q2

R2

q = (q1, q2)

Ω

O

Figure 2: A homeomorphism φ : U → Ω is called a chart.

p = (θ1, θ2) and any number α > 0, a set of all p′ = (θ′1, θ′2)is defined as

Up,α ={p′ :

∥∥p′ − p∥∥H < α

}, (4)

where∥∥p′ − p

∥∥H =

√∑i, j

hi j(p)(θ′i − θi

)(θ′j − θj

)(5)

can be regarded as an open subset of M. Then, the set Mwith such a family of open subsets can be regarded as atopological manifold. It is possible to show that the manifoldM becomes Hausdorff and compact. Further, every point pof M has a neighborhood U that is homeomorphic to anopen subset Ω of 2-dimensional numerical space R2. Sucha homeomorphism φ : U → Ω is called a coordinate chart.In fact, a neighborhood Up,α of posture p with joint angles(θ1, θ2) in Figure 1 can be mapped to an open setΩ in R2 with2 dimensional numerical coordinates (q1, q2) with the originO (Figure 2). In this case, it is possible to see that the originalset M of robot postures can be visualized as a torus shown inR3 (see Figure 3) in which angles q1 and q2 are defined. It isquite fortunate to see that, in the case of typical robots likethe one shown in Figure 1, the local coordinates (q1, . . . , qn)can be identically chosen as a set of n independent jointangles (θ1, . . . , θn) by setting qi = θi (i = 1, . . . ,n). It is alsointeresting to see that the torus in Figure 3 is made to betopologically coincident with the set of all arm endpointsP = (x, y, z). As discussed in detail in mathematical textbooks [5, 6], the topological manifold (M, p) of such a toruscan be regarded as a differentiable manifold of class C∞.

Now, it is necessary to define a tangent vector to anabstract differentiable manifold M at p ∈ M. Let I be aninterval (−ε, ε) and define a curve c(t) by a mapping c :I → M such that c(0) = p. A tangent vector to M at p isan equivalence class of curves c : I → M for the equivalencerelation ∼ defined by

c ∼ c if and only if, in a coordinate chart (U ,φ) around p,we have (φ ◦ c)′(0) = (φ ◦ c)′(0),

where symbol ()′ means differentiation of () with respect tot ∈ I . This definition of tangent vectors to M at p does not

q2

y

x

z

O

q1(q2 = 0)

q1(q2 = ± π)

Figure 3: A two-dimensional torus is expressed by T2 = S1 × S1.

depend on choice of the coordinate chart at p, as discussed intext books [5, 6]. Let us denote the set of all tangent vectorsto M at p by TpM and call it the tangent space at p ∈ M. Ithas an n-dimensional linear space structure like Rn. We alsodenote the disjoint union of the tangent spaces toM at all thepoints of M by TM and call it the tangent bundle of M.

Now, we are in a position to define a Riemannian metricon a differentiable manifold (M, p) as a mapping gp : TpM×TpM → R such that p → gp is of class C∞ and gp(u, v)for u ∈ TpM and v ∈ TpM is a symmetric positive definitequadratic form

gp(u, v) =n∑

i, j=1

gi j(p)uiv j . (6)

Suppose that M is a connected Riemannian manifold. Ifc : I[a, b] → M is a curve segment of class C∞, we define thelength of c to be

L(c) =∫ ba

∥∥c(t)∥∥dt

=∫ ba

√gc(t)

(c(t), c(t)

)dt,

(7)

where we assume c(t) /= 0 for any t ∈ I and call such acurve segment to be regular. A mapping of class C∞c :[a, b] → M is called a piecewise regular curve segment(for brevity, we call it an admissible curve) if there existsa finite subdivision a = a0 < a1 < · · · < ak = bsuch that c(t) for t ∈ [ai−1, ai] is a regular curve fori = 1, . . . , k. Then, it is possible to define for any pair ofpoints p, p′ ∈ M the Riemannian distance d(p, p′) to bethe infimum of all admissible curves from p to p′. It iswell known [4, 5] that, with the distance function d definedabove, any connected Riemannian manifold becomes ametric space whose induced topology is coincident withthe given manifold topology. An admissible curve c in aRiemannian manifold is said to be minimizing if L(c) ≤ L(c)for any other admissible curve c with the same endpoints. Itfollows immediately from the definition of distance that c isminimizing if and only if L(c) is equal to the distance betweenits endpoints. Further, it is known that if the Riemannianmanifold {M, g} is complete, then for any pair of points pand p′ there exists at least a minimizing curve c(t), t ∈ [a, b],


with c(a) = p and c(b) = p′. If such a minimizing curvec(t) is described with the aid of coordinate chart (U ,φ) asφ(c(t)) = (q1(t), . . . , qn(t)), then q(t) = φ(c(t)) satisfies the2nd-order differential equation

d2

dt2qk(t) +

n∑i, j=1

Γki j(c(t))dqi(t)

dtdq j(t)

dt= 0,

(k = 1, . . . ,n),

(8)

where Γki j denotes Christoffel’s symbol defined by

Γki j =12

n∑h=1

gkh(∂gih∂q j

+∂gjh∂qi

− ∂gi j∂qh

), (9)

and (gkh) denotes the inverse of matrix (gkh). A curve q(t) :I → U satisfying (8) together with φ−1(q(t)) is called ageodesic, and (8) itself is called the Euler-Lagrange equationor the geodesic equation.

Given a C∞-class curve c(t) = I[a, b] → M, the quantity

E(c) = 12

∫ ba

∥∥c(t)∥∥2dt

= 12

∫ bagc(t)

(c(t), c(t)

)dt

(10)

is called the energy of the curve. Then, by applying theCauchy-Schwartz inequality for (7), we have

L(c)2 ≤ 2(b− a)E(c). (11)

Further, the equality of (11) follows if and only if ‖c(t)‖ isconstant. It is also possible to see that if c(t) is a geodesicwith c(a) = p and c(b) = p′, then for any other curve c(t)with the same endpoints, it holds

E(c) = L(c)2

2(b− a)≤ L

(c)2

2(b− a)≤ E

(c). (12)

The equalities hold if and only if c(t) is also a geodesic.Conversely, if c(t) with c(a) = p and c(b) = p′ is a C∞

curve that minimizes the energy and makes gc(t)(c(t), c(t))constant, then c(t) becomes a geodesic connecting c(a) = pand c(b) = p′. In mechanics, E(c) is usually called “actionof c,” and c(t) is considered as the orbit of motion of amultibody system.

3. Riemannian Geometry of Robot Dynamics

Dynamics of a robotic mechanism with n rigid bodiesconnected in series through rotational joints are describedby Lagrange’s equation of motion, as shown in (1). It isimplicitly assumed that the axis of rotation of the first bodyis fixed in an inertial frame and denoted by z-axis that isperpendicular to the xy-plane as shown in Figure 1. If thereis no gravity force affecting motion of the robot, then theequation of motion of the robot can be described by the form

H(q)q +{

12H(q) + S

(q, q

)}q = u, (13)

where u stands for a vector of control torques emanatingfrom joint actuators. This formula is valid for motions ofa revolute joint robot, shown in Figure 1, if it is installedin weightless environment like an artificial satellite, or thegravity term g(q) (included in (1)) can be compensatedby joint actuators through control input u. In general, wecan represent a posture p of the robot as a physical entityby a family of joint angles θi (i = 1, . . . ,n), which can beexpressed by a point Θ = (θ1, . . . , θn) in the n-dimensionalnumerical space Rn. In fact, we can naturally imagine anisomorphism φ : U → Ω, where U ⊂ M, and Ω is anopen subset of Rn. In other words, a local coordinate chartφ(U)(= Ω) can be treated to be identical to U itself, an opensubset of M, by regarding q = (q1, . . . , qn)T (“T” denotestranspose and hence q a column vector) identical to Θ bysetting qi = θi (i = 1, . . . ,n). In this way, the abstractmanifold M as the set of all robot postures can be regardedas an n-dimensional torus Tn as an n-tuple direct product ofS1 : Tn = S1×· · ·×S1. Hence, a robot posture p ∈M can berepresented by a point Θ on Tn and also expressed by a jointvector q in Rn.

From the definition of inertia matrices, H(q) is symmet-ric and positive definite, and the kinetic energy is expressedas a quadratic form

K(q, q

) = 12qTH(q)q. (14)

Hence, the equation of motion of the robot is expressed byLagrange’s equation

ddt

{∂

∂qL(q, q

)}− ∂

∂qL(q, q

) = u, (15)

where L(q, q) = K(q, q), and u stands for a generalizedexternal force vector. It is interesting to note that indifferential geometry, (15) can be described as

∑i

hkiqi +∑i, j

Γik j(q)qiq j = uk, (16)

where Γik j denotes Christoffel’s symbol of the first kinddefined by

Γik j = 12

(∂hjk∂qi

+∂hik∂q j

− ∂hi j∂qk

). (17)

For later use, we introduce another Christoffel’s symbolcalled the second kind as shown in the formula

Γki j =12

n∑l=1

hlk(∂hjl∂qi

+∂hil∂q j

− ∂hi j∂ql

)

= 12

n∑l=1

hlkΓil j ,

(18)

where (hlk) denotes the inverse of (hlk), the inertia matrixH(q) = (hlk). Since (hkl) and (hkl) are symmetric, itfollows that Γik j = Γ jki and Γki j = Γkji. Now, we showthat (13) is equivalent to (16) by bearing in mind that


H(q) = ∑i{∂H(q)/∂qi}qi, and the skew symmetric matrix

S(q, q) is expressed as in (3). In fact, the second term in thebracket () of (17) corresponds to the first term in {} of (3)and the third term of (17) does to the second term in {} of(3). Hence, it follows from (3) that

n∑j=1

sk j qj =

n∑j=1

12

[{∂

∂q j

( n∑i=1

qihki

)}qi

−{

∂

∂qk

( n∑i=1

qihi j

)}q j]

=n∑j=1

n∑i=1

12

{(∂hik∂q j

− ∂hi j∂qk

)}qiq j .

(19)

Substituting this into (16) by comparing the last two terms of(17) with the last bracket {} of (19) results in the equivalenceof (13) to (16). It is easy to see that multiplication of (16) byH−1(q) yields

qk +∑i, j

Γki j qiq j =

∑j

hk juj , k = 1, . . . ,n. (20)

If u = 0, this expression is nothing, but the Euler-Lagrangeequation shown in (8). By this reason, from now on, we usesymbol gi j(q) instead of hi j(q) for the inertia matrix H(q)even when robot dynamics are treated.

Now, on the abstract topological manifold M as a set ofall possible postures of a robot, suppose that a Riemannianmetric is given by a scalar product on each tangent spaceTpM:

〈v,w〉 = gi j(p)viw j , (21)

where v = vi(∂/∂qi) ∈ TpM and w = wj(∂/∂q j) ∈ TpM,and the summation symbol

∑in i and j is omitted, and

q = (q1, . . . , qn) represents local coordinates. Then, themanifold (M, p) can be regarded as a Riemannian manifoldand becomes complete as a metric space. Then, accordingto the Hopf-Rinow theorem [5], any two points p, q ∈ Mcan be joined by a geodesic of length d(p, q), that is, a curvesatisfying (8) with shortest length, where

d(p, q) = inf∫ ba

∥∥c(t)∥∥dt

= inf∫ ba

√⟨c(t), c(t)

⟩dt

(22)

with c(a) = p and c(b) = q.As discussed in the previous section, geodesics are the

critical points of the energy functional E(c). Further, ageodesic curve c(t) satisfies ‖c(t)‖ = const. In fact, byregarding c(t) = q(t) that is an orbit on Ω, we have

ddt

⟨q, q

⟩ = ddt

(gi j(q(t)

)qi(t)q j(t)

)

= ddt

{qT(t)G(q(t))q(t)

}

= qT(t)G(q)q(t) + qT(t)G(q)q(t) + qT(t)G(q)q(t)

= 0,(23)

where the equivalent expression

G(q)q +{

12G(q) + S

(q, q

)}q = 0 (24)

to (13) with u = 0 is used, G(q) = (gi j(q)), and si j ofS is given in the form of (3) (where hi j = gi j). It is alsoimportant to note that, on a local coordinate chart Ω ⊂ Rn

corresponding to a neighborhood U of p ∈ M, an orbit q(t)parameterized by time t ∈ [a, b] and expressed by a solutionto (20) (where uj is of C∞ in t) should satisfy

n∑j=1

q j(t)uj(t) = ddtE(q(t)), (25)

or

∫ ba

n∑j=1

q j(t)uj(t)dt = E(q(b))− E(q(a)), (26)

as long as q(t) ∈ Ω, where E(q(t)) = (1/2)〈q(t), q(t)〉. Whenu(t) = 0, E(q(t)) = const. and then the curve connectingp = φ−1(q(a)) and p′ = φ−1(q(b)) must be a geodesic.In other words, an inertia-originated movement withoutbeing affected by the gravitational field or any external forcefield produces a geodesic orbit [2]. The most importantly,geodesics together with their length are invariant under anychoice of local coordinates.

Before closing this expository section on robot motionfrom the Riemannian geometry viewpoint, we must empha-size that all the above invariant properties of geodesics ofinertia-originated robot motions result from imaging a setof all robot postures as an abstract Riemannian manifold.Choice of a local coordinates is originally arbitrary. Evenan n-dimensional torus Tn is one of such choice of localcoordinates corresponding to the choice of joint angles q =(q1, . . . , qn)T . At the same time, it is important to note that,in differential geometry, choice of coordinates in the tangentspace TpM is indeterminable or free to choose. However,once a local coordinates system for an n degrees of freedomrobot is chosen by joint angle vector q = (q1, . . . , qn)T ,then the coordinates in the tangent space TpM should bechosen as the vector of joint angular velocities ∂/∂q =(∂/∂q1, . . . , ∂/∂qn) correspondingly to q = (q1, . . . , qn)T ,from which the Riemannian metric gi j(q) is defined throughthe inertia matrix.

4. Constraint Submanifold andHybrid Position/Force Control

Consider an n-DOF robotic arm whose last link is a penciland suppose that the endpoint of the pencil is in contact witha flat surface ϕ(x) = ξ, where x = (x, y, z)T . It is well knownthat the Lagrange equation of motion of the system is writtenas

G(q)q +{

12G + S

}q + g(q) = −λ∂ϕ(x(q))

∂q+ u, (27)


x

y

z

q1

q2

J1

q3J2

J3q4

P

O(J0)

z = ξ

Figure 4: A hand-writing robot with four DOFs whose endpointP(= (x, y)) is constrained on a plane z = ξ.

where ∂ϕ/∂q can be decomposed into ∂ϕ(q)/∂q =JT(q)∂ϕ/∂x and J(q) = ∂x/∂qT . On the constraint manifoldFξ = {p : p ∈M and ϕ(x(p)) = ξ}, let us consider a smoothcurve c(t) : I[a, b] → Fξ that connects the given two pointsc(a) = p and c(b) = p′, where p and p′ belong to Fξ . Thelength of such a curve constrained to Fξ is defined as

L(c) =∫ ba

√gi j(c(t))ci(t)c j(t)dt, (28)

and consider the minimization

d(p, p′

) = infc∈Fξ

L(c) (29)

that should be called the distance between p and p′ on theconstraint manifold. Then, the minimizing curve called thegeodesic denoted identically by q(t)(= c(t)) must satisfy theEuler equation

qk(t) + Γki j qi(t)q j(t) = −λ(t) · (gradϕ(x(t))

)k, (30)

together with the constraint condition ϕ(x(t)) = ξ, where

gradϕ(x(t)) = G−1(q(t))JT(q)∂ϕ

∂x, (31)

and JT(q) = ∂xT/∂q. It should be noted that, from the innerproduct of (30) and w = JT∂ϕ/∂x, it follows that

∑k

(wkqk +wkΓki j q

iq j) = −λ∑

k

wkgk jw j . (32)

Since the holonomic constraint ϕ(x(q)) = ξ implies 〈w, q〉 =0, it follows that

4∑k=1

(wkqk +

(ddtwk)qk)= 0. (33)

Substituting this into (32), we obtain

λ(t) = 1wTG−1w

{∑k

(wkqk −

∑i, j

wkΓki j qiq j)}

. (34)

From the Riemannian geometry, the constraint force λ(t)with the grad {ϕ(x(q))} stands for a component of the imagespace of w(= JT(q)∂ϕ/∂x) that is orthogonal to the kernelTFξ of w. In other words, this component is cancelled outby the image space component of the left hand side of (32).From the physical point of view, λ(t) should be regarded asa magnitude of the constraint force that presses the surfaceϕ(x(q)) = ξ in its normal direction. In order to compromisethe mathematical argument with such physical reality, letus suppose that the joint actuators can supply the controltorques

u = λd · JT(q)∂ϕ

∂x+ g(q). (35)

Then, by substituting this into (27), we obtain the Lagrangeequation of motion under the constraint ϕ(x(q)) = ξ:

G(q)q +{

12G + S

}q = −(λ− λd)JT(q)

∂ϕ

∂x

= −ΔλJT(q)∂ϕ

∂x,

(36)

where Δλ = λ − λd. It should be noted that introductionof the first term of control signal of (35) does not affectthe solution orbit on the constraint manifold, and further itkeeps the constraint condition during motion by renderingλ(t)(= λd+Δλ(t)) positive. In a mathematical sense, exertionof the joint torque λdJT(∂ϕ/x) plays a role of “pressing” or“lifting” of the image space spanned from the gradient of theconstraint equation. Further, note that (36) is of an implicitfunction form with the Lagrange multiplier Δλ. To affirm theargument of treatment of the geodesics through this implicitform, we show an explicit form of the Lagrange equationexpressed on the orthogonally projected space (kernel space)by introducing the orthogonal transformation

q = (P,w‖w‖−1)(ηz

)= Qq, (37)

where P is a 4×3 matrix whose column vectors with the unitnorm are orthogonal to w, and η denotes a 3 × 1 matrix (3-dim. vector) and z a scalar. Since Q is an orthogonal matrix,Q−1 = QT . Hence, if q ∈ ker(w) (= TFξ), then z = 0.Restriction of (36) to the kernel space of w can be attainedby multiplying (36) by PT from the left such that

PTG(q)ddt

(Pη) + PT{

12G + S

}Pη = 0, (38)

which is reduced to the Eular equation in η:

G(q)η +{

12G + S

}η = 0, (39)

or equivalently

ηk + Γki j η

iη j = 0, k = 1, . . . ,n− 1(= 3), (40)

whereG(q) = PTG(q)P, Γki j the Christoffel symbol for (gi j) =

G, and

S = PTSP − 12PTGP +

12PTGP, (41)


which is skew symmetric, too. Note that the transformationQ is isometric, and (40) stands for the geodesic equation onthe constraint Riemannian submanifold.

5. Hybrid Control for Redundant Systems andStability on a Submanifold

Let us now reconsider a hybrid position/force feedbackcontrol scheme, which is of the form

u = g(q) + λdJT(q)

∂ϕ

∂x+ Cq + JT(q)

(ζ√kx + kΔx

), (42)

where ϕ(x) = z, x = (x, y, 0)T , Δx = (x − xd, y −yd, 0). This type of hybrid control is an extension of theMcClamroch and Wang’s method [7, 8] to cope with arobotic system that is subject to redundancy in DOFs. Fromnow on, we redefine the Jacobian matrix J(q) as 2 × 4matrix J(q) = (∂x/∂qT , ∂y/∂qT) because we consider aspecial case of ϕ(x) = z, that is, the constraint z − ξ = 0,and solution trajectories q(t) that satisfy z(q(t)) − ξ = 0.In relation to this, denote also the two-dimensional vector(x, y) by x and (Δx,Δy) by Δx. It should be noted thatthe orthogonality relationship among (x, y, z) coordinates inE3 does not imply the orthogonality among ∂x/∂q, ∂y/∂q,and ∂z/∂q(= ∂ϕ/∂q). Therefore, ∂z/∂q is not orthogonal toJT(q)(= (∂x/∂q, ∂y/∂q)), and hence the last term of the righthand side of (42) contains both components of image (w)and ker(w).

Now, substituting this into (27) yields

G(q)q +{

12G + S + C

}q + JT(q)

{ζ√kx + kΔx

}

= −Δλ∂z(q)∂q

.(43)

Evidently the inner product of (43) and q under theconstraint z(q) = ξ leads to

ddtE(q, q

) = −qCq − ζ√k∥∥x∥∥2

, (44)

where

E(q, q

) = 12qTG(q)q +

k

2‖Δx‖2. (45)

Unfortunately, this quantity is not positive definite in thetangent bundle TFξ . Nevertheless, it is possible to see thatmagnitudes of q(t) and Δx(t) remain small if at initial timet = 0 both magnitudes ‖q(0)‖ and ‖Δx(0)‖ are set small.Next, let us introduce the equilibrium manifold defined bythe set

EM1 ={q : z(q) = ξ, x(q) = xd, y(q) = yd

}, (46)

which is of one dimension. Fortunately as discussed in thepaper [9], it is possible to confirm that a modified scalarfunction

Wα = E(q, q

)+ αqTG(q)PϕJT(q)Δx (47)

becomes positive definite in TFξ with an appropriate positiveparameter α > 0, provided that the physical scale of therobot is ordinary, and feedback gains C and k are chosenadequately. Here, in (47), Pϕ is defined as I4 − wwT/‖w‖2.Furthermore, if in a Riemannian ball in Fξ defined asB(q(0), r0) = {q : d(q, q(0)) < r0}, the Jacobian matrixJ(q) is nondegenerate, then it can be expected that there existpositive numbers σm and σM such that

σMI2 ≥ JJT ≥ JPϕJT ≥ σmI2. (48)

Further, to simplify the argument, we choose C = cI4 with aconstant c > 0. Then, it follows from differentiation of V(=qTG(q)PϕJT(q)Δx) that

V= −{ζ√kx + kΔx}TJPϕJ

TΔx

− cqTPϕJTΔx + qTGPϕJT x− h(q,G)Δx,

(49)

where

h(q,G) = qT{(

12GPϕ − SPϕ −GPϕ

)JT −GPϕJT

}. (50)

This 1×2-vector h is quadratic in components of q, and eachcoefficient of qiq j (for i, j = 1, . . . , 4) is at most of order ofthe maximum spectre radius of G(q) denoted by gM . Hence,it follows from (49) that

∣∣h(q,G)Δx∣∣ ≤ gMl0

∥∥q∥∥2‖Δx‖, (51)

where l0 signifies a constant that is of order of the maximumlink length (because the Jacobian matrix ∂x/∂qT is homoge-neously related to the three link lengths of the robot shownin Figure 4). Further, note that qTPϕ = 0 in this case andremark that

qTGPϕJT x ≤ 1

2qTGGq +

12

xTJPϕJT x, (52)

−ζ√kxTJPϕJTΔx ≤ ζk

2ΔxTJPϕJTΔx +

ζ

2xT(JJT)

x. (53)

Thus, substituting (51) to (52) into (49) yields

V ≤ −{k − 1

2− ζk

2

}ΔxTJPϕJTΔx

+12qTGGq +

1 + ζ

2xTJJT x + gMl0

∥∥q∥∥2‖Δx‖.(54)

With the aid of a positive parameter α > 0, it is easy to seethat

Wα ≥ E − α

2qTG(q)G(q)q − α

2ΔxTJPϕJTΔx

= 12qT{G− αGG}q +

12ΔxT

(kI2 − αJPϕJT

)Δx.

(55)

Now, suppose that the robot has an ordinary physicalscale with

gM ≤ 0.001 [kgm2], l0 ≤ 0.2 [m]. (56)


EM1

Geodesic Orbit

Fξ

B(q(0), r0)

r0

q∗ε q∞

q(0) δ

Figure 5: Definition of the Riemannian ball B(q∗, δ). Any orbitstarting from B(q∗, δ) converges to EM1 ∩ B(q∗, ε).

Then, it is possible to see that

Wα = E + αV ≤ −αkσm2‖Δx‖2 − c

3

∥∥q∥∥2, (57)

provided that ‖Δx(t)‖ < 0.3 [m] , (1 + ζk) ≤ k, and ζ√k >

α(1 + ζ)/2. From the practical point of view of control designfor the handwriting robot, ζ is set around ζ = √2/2, and k ischosen between 5.0 ∼ 20.0 [kg/s2] (see [9]). On account of(48) and (56),

Wα ≥(1− αγ0

)E, (58)

and similarly,

Wα ≤(1 + αγ0

)E, (59)

where γ0 = max(gM , σM/k). Then, if the damping factor canbe selected to satisfy both inequalities c ≥ (3/2)(ασmgM) andc > αgMl0, then (57) is reduced to

Wα ≤ −ασm1 + αγ0

Wα, (60)

where γ0 can be set as γ0 = σM/k which is larger than gM .Hence, by choosing α = 1/2γ0 = k/2σM , it follows from (60)and (59) that

Wα(t) =Wα(0)e−γt, (61)

where γ = kσm/3σM .Now, suppose that q∗ in EM1∩B(q(0), r0) is the minimiz-

ing point that connects with q(0) among all geodesics fromq(0) to any point of EM1∩B(q(0), r0). We call q∗ a referencepoint corresponding to q(0).

Definition 1 (stable Riemannian ball on a submanifold). Iffor any ε > 0, there exists the number δ(ε) > 0 such that anysolution trajectory (orbit) of (43) starting from an arbitraryinitial position inside B(q∗, δ(ε)) with q(0) = 0 remainsinside B(q∗, ε) for any t > 0, then the reference point q∗ onEM1 is said to be stable on a submanifold (see Figure 5).

It can be concluded from the exponential convergenceof Wα to zero and the inequality E ≤ 2Wα when α =1/2γ0 that any point inside B(q∗, δ(ε)) included in B(q(0), r1)

for some r1(< r0) is stable on a submanifold and furthersuch a solution trajectory converges asymptotically to someq∞ on the equilibrium manifold EM1 in an exponentialspeed of convergence. This can be well understood asa natural extension of the well-known Dirichlet-Lagrangestability under holonomic constraints to a system withDOF-redundancy. The details of the proof are presented inAppendix A.

It should be noticed from the proof in Appendix A thatasymptotic convergence of the solution trajectory to someq∞ on the equilibrium manifold implies also the asymptoticconvergence of constraint force λ(t) to λd as t → ∞ becauseΔλ = λ− λd is expressed as

Δλ(t) = 1wTG−1w

{∑k

(wqk −

∑i, j

wkΓki j qiq j)

−wTJT(ζ√kx + kΔx

)},

(62)

and this right hand side converges to zero as t → ∞.The stability notion of a Riemannian ball in a neighbor-

hood of a reference equilibrium state q∗ on EM1 is extendedto cope with the case that the initial velocity vector q(0) isnot zero. To do this, we define an extended Riemannian ballin the tangent bundle M × TFξ around (q∗, q = 0) in such away that

B{(q∗, 0

);(r0, rK

)}

={(q, q

): d(q, q∗

)< r0,

√(1/2)qTG(q)q < rK

},

(63)

where d(q, q∗) denotes the distance between q and q∗

restricted to the submanifold Fξ , andG is defined below (40).

Definition 2 (asymptotic stability on a submanifold). If forany ε > 0, there exist numbers δ(ε) and δK (ε) such that anysolution trajectory of (43) starting from an arbitrary initialposition and velocity inside B{(q∗, 0); (δ(ε), δK (ε))} remainsin B{(q∗, 0); (ε, rK )} and further converges asymptotically tosome equilibrium point q∞ ∈ EM1 with still state, thenthe reference point with its posture on EM1 is said to beasymptotically stable on a constraint submanifold.

It should be remarked that Bloch et al. [10] introducesoriginally the concept of stabilization for a class of nonholo-nomic dynamic systems based upon a certain configurationspace. The redefinition of stability concepts introducedabove is free from any choice of configuration spaces (localcoordinates) and assumptions on an invertibility condition(that is almost equivalent to nonlinear control based oncompensation for nonlinear inertia-originated terms). Liuand Li [11] also gave a geometric approach to modelingof constrained mechanical systems based upon orthogonalprojection maps without deriving a compact explicit formof the Euler equation like (40) with a reduced dimensiondue to constraints. Therefore, the proposed control schemewas developed on the basis of compensation for the inertia-originated nonlinear terms (that is almost equivalent to


q12

q11

O

y

O′x

q13O01

ϕ1

e1

Q1

P1

n1

Q2

P2

n2

e2

Om

rX

rY ϕ2

Y

Xq22

q21

O02

θ

Figure 6: Two-dimensional object grasping by a pair of multijointrobot fingers.

y

c(s) θ

Q1

Om

X

rX

P1 = (X(s),Y(s))

θ1

s = 0

n1

Object

Y

rY

Y(s)Om = (xm, ym)

θ1

e1

P1

P1 = (x1, y1)

O01 = (x01, y01)

Finger-tip

Figure 7: Definitions of physical symbols in relation to rollingcontact. θ1 denotes θ1(s) appearing in (64).

the computed torque method). A naive idea of stability ona manifold by using different metrics for the constrainedsubmanifold and its tangent space was first presented in [9]and used in stabilization control of robotic systems with DOFredundancy [12, 13].

6. 2-dimensional Stable Grasp of a Rigid Objectwith Arbitrary Shape

Consider a control problem for stable grasping of a 2Drigid object by a pair of planar multijoint robot fingers withhemispherical fingertips as shown in Figure 6. In this figure,

the two robots are installed on the horizontal xy-plane E2.We denote the object mass center byOm with the coordinates(xm, ym) expressed in the inertial frame. On the other hand,we express a local coordinate system fixed at the object byOm-XY together with unit vectors rX and rY along theX-axisand Y-axis, respectively (see Figure 7). The left-hand sidesurface of the object is expressed by a curve c(s) with localcoordinates (X(s),Y(s)) in terms of arc length parameter s asshown in Figure 7.

First, suppose that at the left-hand contact point P1 thefingertip of the left finger is contacting with the object.Denote the unit normal at P1 by n1 and the unit tangentvector by e1. Note that n1 is normal to both the object surfaceand finger-end sphere at P1, and e1 is tangent to them at P1,too. If we denote position P1 by local coordinates (X(s),Y(s))fixed at the object (see Figure 7), then the angle from the X-axis to the unit normal n1 is assumed to be determined by afunction on the curve:

θ1(s) = arct

(X ′(s)Y ′(s)

), (64)

where X ′(s) = dX(s)/ds and Y ′ = dY(s)/ds. In this paper, allangles are set positive in counterclockwise direction. Then,

P1P′1 = ln1(s) = −X(s) cos θ1(s) + Y(s) sin θ1(s), (65)

which is dependent only on s and, therefore, a shape functionof the object. On the other hand, this length can be expressedby using the inertial frame coordinates in the following way(see Figure 8):

P1P′1 =

(xm − x01

)cos

(θ + θ1(s)

)− (ym − y01

)sin(θ + θ1(s)

)− r1.(66)

Hence, the left-hand contact constraint can be expressed bythe holonomic constraint

Q1 = −(xm − x01

)cos

(θ + θ1

)+(ym − y01

)sin(θ + θ1

)+(r1 + ln1(s)

)= 0,

(67)

where ln1(s) denotes the right-hand side of (65) and θ1 =θ1(s) for abbreviation.

Next, we note that the length OmP′1 can be regarded also

as a shape function of the object given by

OmP′1 = le1(s) = X(s) sin θ1 + Y(s) cos θ1. (68)

On the other hand, this quantity can be also expressed as

OmP′1 = Y1(t)

= −(xm − x01)

sin(θ + θ1

)− (ym − y01)

cos(θ + θ1

).

(69)

As discussed in [13], in the light of the book [14], purerolling at P1 is defined by the condition that the translationalvelocity of P1 on the finger sphere is equal to that of P1


O01P1

r1

S1 s = 0

e1

θ1

Y ′

θ1

n1

θ

ln1(s)

θ + θ1

X(s)

le1(s)

θ

OmX

Y(s)

P′′1

θ1

P′1

x

e′1y

Y1(t)

Y

Figure 8: Physical and geometrical meanings of key variables ln1(s)and le1(s) defined in relation to the object contour c(s) expressed interms of length parameter s of the curve.

on the object contour along the e1-axis. Hence, by denotingp1 = q11 + q12 + q13, we have

− r1 p1 + r1θ

= −Y1

= (xm − x01)

sin(θ + θ1

)+(ym − y01

)cos

(θ + θ1

)+{(xm − x01

)cos

(θ + θ1

)− (ym − y01)

sin(θ + θ1)

}θ,

(70)

which from (67) can be reduced to

− r1 p1 − ln1θ +(x01 − x

)sin(θ + θ1

)+(y01 − y

)cos

(θ + θ1

) = 0.(71)

This constraint form is Pfaffian. As to the contact pointP2 at the right-hand finger end, a similar nonholonomicconstraint can be obtained. Thus, by introducing Lagrange’smultipliers f1 and f2 associated with holonomic constraintsQi = 0 (i = 1, 2), it is possible to construct a Lagrangian:

L = 12

∑qTi Gi

(qi)qi +

12

{Mx2 +My2 + Iθ2}

− f1Q1 − f2Q2,(72)

where qi denote the joint vector for finger i, Gi(qi) the inertiamatrix for finger i, M and I denote the mass and inertiamoment of the object. Since both the rolling constraints arePfaffian, it is possible to associate (71) and its correspondingform at P2 with another multipliers λi (i = 1, 2) and regardthem as external forces. Thus, by applying the variationalprinciple to the Lagrangian together with the external forces,

we obtain the Lagrange equation of motion of the overallfingers-object system:

Iθ − f1Y1 + f2Y2 − λ1ln1 + λ2ln2 = 0, (73)

M

(xy

)− f1n1 − f2n2 − λ1e1 − λ2e2 = 0, (74)

Gi(qi)qi +

{12Gi + Si

}qi + fiJ

Ti

(qi)

ni

+ λi{JTi(qi)

ei + (−1)iripi} = ui, i = 1, 2,

(75)

where p1= (1, 1)T , p2= (1, 1, 1)T , and JTi (qi)=∂(x0i/ y0i)/∂qi,and

ni = (−1)i(− cos

(θ + θi

)sin(θ + θi

))

, ei =(

sin(θ + θi

)cos

(θ + θi

))

(76)

for i = 1, 2. It is important to note that the object dynamicsof (73) and (74) can be recast in the form

H0z + f1w1 + f2w2 + λ1w3 + λ2w4 = 0, (77)

where z = (x, y, θ), and

wi =(−ni

(−1)iYi

), wi+2 =

(−ei

(−1)ilni

), i = 1, 2, (78)

which are of a two-dimensional wrench vector. This impliesthat if the sum of all wrench vectors converges to zero, thenthe force/torque balance is established. Further, if we define

X = (x, y, θ, qT1 , qT2)T

,

u = (0, 0, 0,uT1 ,uT2)T

,

G(X) = diag(M,M, I ,G1

(q1),G2

(q2))

,

S(X, X) = diag(0, 0, 0, S1, S2

),

(79)

then (73) ∼ (75) can be written in the form

G(X)X +{

12G + S

}X +

∑i=1,2

(fi∂Qi

∂X+ λi

∂Ri∂X

)= u, (80)

where

Ri = −(−1)iri{(θ + θi

(si))− pTi qi

}+ Yi + ψi

(si), (81)

and ψi signifies a function depending only on the shapeparameter si. The details of the integral form of (81) willbe discussed in Appendix B. Comparing (80) with (27), itmust be understood that a similar but extended argumentof Section 4 can be applied even for a case of physicalinteraction with complex holonomic constraints and non-holonomic (but Pfaffian) constraints. More precisely, whenu = 0 in (80), the image space of the Riemannian manifold(X,G(X)) with constraints Q1 = 0 and Q2 = 0 is spannedfrom the gradients

G−1(X)

(∂Q1

∂X,∂Q2

∂X,∂R1

∂X,∂R2

∂X

), (82)


and the kernel space should be defined as the orthogonalcompliment to the image space. Note that the rollingconstraints of (71) and another for i = 2 induce furtherrestriction of the tangent space to a more subdimensionallinear space. Here, ∂Ri/∂X should be taken at the time instantt under the condition that si are fixed. Nevertheless, as shownin Appendix B, it is quite interesting to know that Ri fori = 1, 2 can be regarded as a constant function in changeof t even under any infinitesimally small variation of si, thatis, dRi/dt = 0 for i = 1, 2. In other words, Ri = constant (i =1, 2), and these expressions imply a holonomic constraint.Such a geometric structure of decomposition of the tangentspace to an image space of constraint gradients and itsorthogonal compliment is known in general mathematicalterminology (see [15, 16]) but has not yet been criticallyspelled out in the case of existence of rolling contactconnections between curved rigid bodies.

The Euler equation of (80) can be expressed in a similarform to (1) by introducing the constraint vector Φ and thevector of Lagrange multipliers λ as

Φ = (Q1,R1,Q2,R2), λ = ( f1, λ1, f2, λ2

)T. (83)

Then, (80) can be written in the form

G(X)X +{

12G + S

}X +

∂Φ

∂Xλ = u, (84)

which must be valid for the constraint XT(∂Φ/∂X) = 0. Wedenote the (n−4)-dimensional kernel space of ∂Φ/∂X by VX

and its 4-dimensional orthogonal compliment as the imagespace of G−1(X)∂Φ/∂X by HX.

We are now in a position to derive an update law of thelength parameters s1 and s2 along the object contours. Firstly,it is important to verify that the holonomic constraintQ1 = 0is invariant under any infinitesimally small variation of s1. Infact, we see from (64) to (69) that

∂Q1

∂s= {(xm − x01

)sin(θ + θ1

)+(ym − y01

)cos

(θ + θ1

)}

× ∂θ1

∂s+∂

∂sln1(s)

= −le1(s)∂θ1(s)∂s

+{− X ′(s) cos θ1 + Y ′(s) sin θ1

}

+{X(s) sin θ1 + Y(s) cos θ1

}∂θ1(s)∂s

= −le1(s)∂θ1

∂s+ 0 + le1(s)

∂θ1

∂s= 0,

(85)

where we denote s1 = s for simplicity. Next, it is possible toshow that if θ1(s) is variable, then θ′1(s) = dθ1(s)/ds = κ1(s),where κ1(s) is the curvature of the left-hand contour of theobject. Then, it is obvious from Figures 6 to 8 that

r1(− Δp1 + Δθ + Δθ1

)Δt

= −Δs = −κ−11 (s)

Δθ1

Δt, (86)

where κ1(s) /= 0. Since θ′1(s) = κ1(s), (86) is reduced to

r1(− Δp1 + Δθ

)Δt

= −(1 + r1κ1(s))ΔsΔt. (87)

This equation should be accompanied with the Euler-Lagrange equation (80). This can be also regarded as anonholonomic constraint to (80). As to the right-handcontour, it follows that

ds2dt

= −r2

1 + r2κ2(s2)(− p2 + θ

). (88)

7. Lifting in Horizontal Space andForce/Torque Balance

First, in order to find an adequate lifting that belongs to theimage space HX and realizes the force/torque balance (see(77)) in the sense that

f1dw1 + f2dw2 + λ1dw3 + λ2dw4 = 0, (89)

we remark that(xm − x01

ym − y01

)= Rθ+θ1

(r1 + ln1

(s1)

−Y1

)(90)

−(xm − x02

ym − y02

)= Rθ+θ2

(ln(s2)

+ r2

Y2

), (91)

where

Rθ+θ1 =(

n1(θ), e1(θ))

=(

cos(θ + θ1) sin

(θ + θ1

)− sin

(θ + θ1) cos

(θ + θ2

)).

(92)

We also define

Rθ+θ2 =(

cos(θ + θ2

)sin(θ + θ2

)− sin

(θ + θ2

)cos

(θ + θ2

))

,

Rθ =(

cos θ sin θ− sin θ cos θ

).

(93)

Then, it follows from (90) and (91) that

−(x01 − x02

y01 − y02

)= Rθ

{Rθ1

(r1 + ln1

(s1)

−Y1

)

+Rθ2

(r2 + ln2

(s2)

Y2

)}

= Rθ

(l(

s,Y1,Y2)

−d(s,Y1,Y2))

,

(94)

where s = (s1, s2) and

l(

s,Y1,Y2) = (r1 + ln1

)cos θ1 +

(r2 + ln2

)cos θ2

− Y1 sin θ1 + Y2 sin θ2,

d(

s,Y1,Y2) = (r1 + ln1

)sin θ1 +

(r2 + ln2

)sin θ2

+ Y1 cos θ1 − Y2 cos θ2.

(95)

Thus, let us define

fid =fd

r1 + r2

{l cos θi + d sin θi

},

λid = (−1)ifd

r1 + r2

{− l sin θi + d cos θi}

,i = 1, 2, (96)


and remark that they satisfy

f1dn1 + f2dn2 + λ1de1 + λ2de2 = 0. (97)

Further, note that

f1dw1 + f2dw2 + λ1dw3 + λ2dw4 = (0, 0, SN )T , (98)

where

SN =fd

r1 + r2

{− (l cos θ1 + d sin θ1)Y1

+(l cos θ2 + d sin θ2

)Y2

− (l sin θ1 − d cos θ1)ln1

− (l sin θ2 − d cos θ2)ln2}

= fdr1 + r2

{l(r1 sin θ1 + r2 sin θ2)

− d(r1 cos θ1 + r2 cos θ2)}.

(99)

This shows according to (98) that if SN tends to vanish, thenthe force/torque balance is established, that is, the total sumof wrench vectors exerted to the object becomes zero.

8. Control Signal for Blind Grasping anda Morse-Lyapunov Function

From the practical standpoint of designing a control signalfor stable grasping, it is important to see that objects tobe grasped are changeable, but the pair of robot fingers isalways the same. That is, for designing control signals, we areunable to use physical parameters of the object such as thelocation (xm, ym) of its mass center and object geometry. Onthe contrary, it is possible to assume the knowledge of fingerkinematics like finger link lengths and locations of the centersof finger-end spheres and to use measurement data of fingerjoint angles and angular velocities. In view of these points, letus propose a family of control signals defined as

ui = −ciqi + (−1)ifd

r1 + r2JTi(qi)(x01 − x02

y01 − y02

)− riNipi,

i = 1, 2,(100)

where

Ni(t) = γ−1i ripTi

(qi(t)− qi(0)

), (101)

and p1= (1, 1, 1)T , p2= (1, 1)T , ci denotes a damping factor,and γi > 0 a positive gain specified later.

The closed loop dynamics of motion of the overallfingers-object system can be derived by substituting ui of(100) into (75) for i = 1, 2. In order to spell out the dynamics

in a more physically meaningful way for later discussions,first note the following three equalities:

12

{(x01 − x02

)2+(y01 − y02

)2} = 12

(l2 + d2)

fid∂Qi

∂qi+ λid

∂Ri∂qi

= (−1)i[JTi

{− fid

(cos

(θ + θi

)− sin

(θ + θi

))

+λid

(sin(θ + θi

)cos

(θ + θi

))}

+ λidripi

]

= (−1)i fdr1 + r2

{−JTi Rθ

(l−d

)+ λidripi

}

= (−1)ifd

r1 + r2

{JTi

(x01 − x02

y01 − y02

)+ λidripi

}, i = 1, 2,

(102)

Then, by substituting (98), (102) into (73) to (75), it ispossible to express the closed loop dynamics in the followingforms:

Iθ − Δ f1Y1 + Δ f2Y2 − Δλ1ln1 + Δλ2ln2 − SN = 0,

M

(xy

)+ Δ f1n1(θ) + Δ f2n2(θ)− Δλ1e1(θ)− Δλ2e2(θ) = 0,

Giqi +{

12Gi + Si

}qi + ciqi + Δ fiJ

Ti ni(θ)

+ Δλi{JTi ei(θ) + (−1)iripi

}+ ri

{(−1)iλid + Ni

}pi = 0, i = 1, 2.

(103)

Similarly, to the form of (84), these equations can be writtenin the following way:

G(X)X +{

12G + S + C

}X +

∂Φ

∂XΔλ

+ SNbθ +∑i=1,2

{Ni + (−1)iλid

}bi = 0,

(104)

where

Δλ = ( f1 − f1d, f2 − f2d, λ1 − λ1d, λ2 − λ2d)T

,

bθ= (0, 0, 1, 0, 0, 0, 0, 0)T ,

b1= (0, 0, 0, 1, 1, 1, 0, 0)T ,

b2= (0, 0, 0, 0, 0, 0, 1, 1)T .

(105)

At this stage, it is important to note that in accordance withfour constraints Φ = 0, the velocity vector X belongs tothe kernel space of (∂Φ/∂X)T and, therefore, XT∂Φ/∂X = 0.Further by using (100) and taking inner product of (84) andX, we obtain

ddt

{K +

fd2(r1 + r2

){(x01 − x02)2

+(y01 − y02

)2}

+∑i=1,2

γi2N2i

}= −

∑i=1,2

ci∥∥qi∥∥2

,

(106)


where K denotes the system’s kinetic energy defined by

K =∑i=1,2

12qTi Gi

(qi)qi +

M

2

(x2 + y2) +

I

2θ2. (107)

The relation of (106) must be equivalently derived by takinginner product of X and the closed loop dynamics of equation(104). To verify this, let us define

p1 = q11 + q12 + q13 = pT1 q1, p2 = q21 + q22 = pT2 q2,(108)

P = fd2(r1 + r2

){l2(s,Y1,Y2)

+ d2(s,Y1,Y2)}

+∑i=1,2

γi2N2i ,

(109)

where l and s are defined in (95). It should be remarkedthat, with the aid of expressions of integral form of rollingconstraints shown in (71), we have

∂l

∂θ= −∂Y1

∂θsin θ1 +

∂Y2

∂θsin θ2

= −r1 sin θ1 − r2 sin θ2,

∂d

∂θ= r1 cos θ1 + r2 cos θ2,

(110)

and hence,

∂P

∂θ= fdr1 + r2

{l∂l

∂θ+ d

∂d

∂θ

}

= − fdr1 + r2

{l(r1 sin θ1 + r2 sin θ2

)

− d(r1 cos θ1 + r2 cos θ2)}

= SN .

(111)

Similarly, from (66), (69), and (101), we have

∂P

∂pi= fdr1 + r2

{l∂l

∂pi+ d

∂d

∂pi

}+ γiNi

∂Ni

∂pi

= (−1)ifd

r1 + r2

{l∂Yi∂pi

sin θi − d ∂Yi∂pi

cos θi

}+ γiNi

∂Ni

∂pi

= −ri fdr1 + r2

{l sin θi − d cos θi

}+ riNi

= (−1)iriλid + riNi, i = 1, 2.(112)

Thus, the inner product of X and (104) or the relation of(106) is reduced to

ddt{K + P} = −

∑i=1,2

ci∥∥qi∥∥2

. (113)

In other words, the closed loop dynamics of (104) can beexpressed in the form

G(X)X +{

12G + S + C

}X +

∂P

∂X+∂Φ

∂XΔλ = 0. (114)

This is interpreted as a Lagrange equation of the Lagrangian

L = K − P −ΦΔλ, (115)

in accompany with the external damping torques ciqi for i =1, 2 through finger joints. The scalar function P defined by(109) is a quadratic function of Y1, Y2, p1, and p2, and hence,it is regarded as a quadratic function of θ, p1, and p2 sinceYi can be regarded as a linear function of θ and pi for i =1, 2 because of (71). Hence, P can be regarded as a Morsefunction defined on the Riemannian submanifold inducedby two constraints described by Qi = 0 for i = 1, 2 and fourconstraints in the tangent space described by Qi and Ri = 0for i = 1, 2.

9. Physical Insights into Gradient andHessian of the Morse Function

The physical meaning of control signals for blind graspingdefined by (100) is quite simple. The first term of the right-hand side of (100) plays a role of damping for rotationalmotion of finger joints. Damping for motion of the objectis exerted from velocity constraints Qi = 0 and Ri = 0 fori = 1, 2 as discussed in detail in the previous paper [1]. Thesecond term plays a role of fingers-thumb opposition thatinduces minimization of the distance between O01 and O02,centers of finger-end spheres. The distance is equivalent to√l2 + d2 as discussed in Section 5. The third term plays an

important role in suppressing excess movements of rotationof finger joints. These characteristics of the control signalcondense into the Lagrange equation of motion with (1) thepotential P(s, θ, p1, p2) of (109), (2) the lifting (∂Φ/∂X)λd,where λd = ( f1d, f2d, λ1d, λ2d)T , and (3) the gradient ∂P/∂Xof the potential. In other words, (113) implies that ifthe artificial potential P attains its minimum and, at thesame time, makes the gradient ∂P/∂X vanish at some s =s∗ (s = (s1, s2)T), and, moreover, the artificial potential ispositive definite in X under the two holonomic constraints,then the equilibrium position that minimizes P wouldbe asymptotically stable. Unfortunately, P is a quadraticfunction with respect to only θ, p1, and p2, and, therefore, Pis only nonnegative definite in X. Nevertheless, it is possibleto show that the Hessian matrix of P with respect to θ, p1,and p2 becomes positive definite in these three variables asshown in Table 1 that is calculated by partially differentiatingthe gradients ∂P/∂θ, ∂P/∂p1, and ∂P/∂p2 of (111) to (112)with respect to θ, p1, and p2 again. Apparently, the Hessianbecomes positive definite provided that γi is chosen as beingof similar order of (ri/ fd), and θi(si) remains in a region suchthat |θi(si)| < π/6 for i = 1, 2. Then, by using a similarargument used in proving the stability on a submanifold forDOF redundant systems [9, 13], a solution trajectory to theLagrange equation converges exponentially to the constraintequilibrium manifold. In this paper, we omit the details ofthe argument, but show a physical meaning of the gradient∂P/θ together with ∂P/∂pi (i = 1, 2).

As shown in Figure 9, all rotational motions of the objectemerge around axes that are perpendicular to the xy-plane.Here, we consider the rotational moment that may emerge


Table 1: Hessian matrix of the potential P.

∂2P

∂θ2= ∂SN

∂θ= fdr1 + r2

{∂l

∂θ

(r1 sin θ1 + r2 sin θ2

)

−∂d∂θ

(r1 cos θ1 + r2 cos θ2

)}

= fdr1 + r2

{(r1 sin θ1 + r2 sin θ2

)2+(r1 cos θ1 + r2 cos θ2

)2}

= fdr1 + r2

{r2

1 + r22 + 2r1r2 cos

(θ1 − θ2

)},

∂2P

∂p2i= r2

i

{fd

r1 + r2+

1γi

}, i = 1, 2,

∂2P

∂pi∂θ= − fd

r1 + r2

{r2i + r1r2 cos

(θ1 − θ2

)}, i = 1, 2,

∂2P

∂p2∂p1= − r1r2 fd

r1 + r2cos

(θ1 − θ2

).

at the contact point P2 exerted by the pressing force f1 to theobject from the other contact point P1. Another force f2 atP2 that presses the object generates the torque around the z-axis at P1. The sum of these torques around the z-axis can beexpressed as

r1 × f1 + r2 × f2

= −−→P1P2 ×r1 fdr1 + r2

n1 +−−→P2P1 ×

r2 fdr1 + r2

n2

=

⎛⎜⎝X(s1)− X(s2)

Y(s1)− Y(s2)

0

⎞⎟⎠× r1 fd

r1 + r2

⎛⎜⎝

cos θ1

− sin θ1

0

⎞⎟⎠

+

⎛⎜⎝X(s2)− X(s1)

Y(s2)− Y(s1)

0

⎞⎟⎠× r2 fd

r1 + r2

⎛⎜⎝− cos θ2

sin θ2

0

⎞⎟⎠

= fdr1 + r2

⎛⎜⎜⎜⎝

00

−(X1 − X2)(r1 sin θ1 + r2 sin θ2

)−(Y1 − Y2

)(r1 cos θ1 + r2 cos θ2

)

⎞⎟⎟⎟⎠ ,

(116)

where we denote Xi = X(si) and Yi = Y(si) for abbreviation.We see also that from geometric relations of the vectors

−−−→OmPi,

and quantities Yi and lni expressed in local coordinates Om-XY (see Figure 9), it follows that

(l−d

)= r1

(cos θ1

− sin θ1

)+ r2

(cos θ2

− sin θ2

)

+ Rθ1

(ln1

−Y1

)+ Rθ2

(ln2

−Y2

)

= r1

(cos θ1

− sin θ1

)+ r2

(cos θ2

− sin θ2

)

−(X(s1)− X(s2)

Y(s1)− Y(s2)

).

(117)

θ

Om

P2

n2n1

P1

Y

Figure 9: The pressing force to the object at P1 in the direction n1

induces a rotational moment around P2 and vice versa.

Applying this relation to (116), we see that

r1 × f1 + r2 × f2

= fdr1 + r2

(0, 0, l

∑i=1,2

ri sin θi − d∑i=1,2

ri cos θi

)T

= (0, 0, SN)T.

(118)

As a summary of the argument, it is possible to concludethat a solution trajectory to (114) converges asymptoticallyto some equilibrium state X = X∗ with some s = s∗ with∂P/∂X = 0 at X = X∗ and s = s∗.

10. Conclusions

A natural extension of hybrid position/force control forrobots with redundant degrees of freedom is presented fromthe standpoint of a Riemannian geometric approach. Itis shown that any supply of the constant pressing forcelying in the image space of the constraint gradient to theenvironment through joint actuations is not relevant tomotions in the kernel space orthogonally compliment tothe image space. An extension of problems of grasping andmanipulation of rigid objects to the case of 2-dimensionalobjects with arbitrary shape is also treated from the view-point of Dirichlet-Lagrange’s stability by introducing a non-negative definite Morse-Lyapunov function on a Riemannianmanifold together with damping shaping.

Appendices

A. A Proof of Stability

From exponential decay of Wα(t) as t → ∞ shown in (61)and (58) and (59) (where α is set as α = 1/2γ0), it follows


that

E(t) ≤ e−γt

1− αγ0Wα(0)

≤ 1 + αγ0

1− αγ0e−γtE(0)

= 3e−γtE(0).

(A.1)

Hence,√E(t) is also exponentially convergent to zero. Since

q(t) is also exponentially convergent in t, q(t) converges tosome q∞ that belongs to the equilibrium manifold EM1.Along the trajectory, the Riemannian distance L(q(0), q(T))must satisfy

L(q(0), q(T)

) ≤∫ T

0

√qT(t)G

(q(t)

)q(t)dt, (A.2)

and it is possible to suppose that L(q(0), q(T)) converges toL(q(0), q∞) as T → ∞. On the other hand, for any T > 0, itfollows from (A.1) that

∫ T0

√qT(t)G

(q(t)

)q(t)dt ≤

∫ T0

√2E(t)dt

≤√

6E(0)(

2γ

).

(A.3)

Hence, substituting this into (A.2) yields

R(q(0), q(T)

) ≤√

6E(0)(

2γ

). (A.4)

In particular, we have

L(q(0), q∞

) =√

6E(0)(

2γ

). (A.5)

Thus, it follows that

L(q∗, q(T)

) ≤ L(q(0), q(T)

)+ L(q(0), q∗

)

≤√

6E(0)(

2γ

)+ L(q(0), q∗

).

(A.6)

First, we consider the case that at initial time t = 0 theangular velocity vector is zero, that is, q(0) = 0. Then,since q∗ in EM1 and, therefore, x(q∗) = xd and J(q) isnondegenerate in a local coordinate chart containing q∗,there exists a number δ0(ε) > 0 such that any q(0) satisfying

L(q(0), q∗

)< δ0(ε) (A.7)

implies

∥∥x(q(0))− xd∥∥2√

3kγ

≤ ε

2. (A.8)

Now, let us define

δ(ε) = min{ε

2, δ0(ε)

}. (A.9)

Then, for any q(0) satisfying L(q(0), q∗) < ε/2, it followsfrom (A.6) and (A.8) that

L(q∗, q(T)

) ≤ ε

2+ L(q(0), q∗

)< ε, (A.10)

which completes the proof of stability of the reference pointq∗ on EM1.

B. Integrability of Rolling Constraints

Suppose that the contact point P1 on the left-hand finger endwith the object contour moves along the finger end circlestarting from the initial time t = 0 to the present time t = t(Figure 6). The length of its movement can be expressed by

−{s1(t)− s1(0)} = r1

{ϕ1(t)− ϕ1(0)}

= r1[{θ(t) + θ1

(s1(t)

)− p1(t)}

− {θ(0) + θ1(s1(0)

)− p1(0)}]

,

(B.1)

that is equivalent to the formula of (86). We show that ψi(si)for i = 1, 2 can be chosen as

ψi(si) = (−1)i

{lei(si)

+ si}

, i = 1, 2. (B.2)

In fact, let us define in accordance with (81) and (B.2)

Ri = ri{θ + θi

(si)− pi

}− (−1)i{Yi − lei

(si)}

+ si (B.3)

for i = 1, 2 and take the derivative of them in t. Then, itfollows that

dRidt

= ri{θ − pi

}− (−1)i{∂Yi∂qTi

qi +∂Yi∂xT

x +∂Yi∂θ

θ

}

+∂{riθi − (−1)i

(Yi − lei

)}∂si

dsidt

+dsidt.

(B.4)

Note that from the meanings of (64) to (69), it follows that

∂lei∂si

= −(−1)i[{X ′(si)

sin θi + Y ′(si) cos θi

}

+{X(si)

cos θi − Y(si)

sin θi}∂θi∂si

]

= (−1)ilni(si)∂θi∂si

− (−1)i,

∂Yi∂si

= (−1)i{(xm − x0i

)cos

(θ + θi

)

+(ym − y0i

)sin(θ + θi

)}∂θi∂si

= (−1)i{ri + lni

(si)}∂θi∂si

.

(B.5)

Substituting these two equations into (B.4) and noticingequality (70), we have now shown that

dRidt

= 0, i = 1, 2. (B.6)

C. Frenet-Serret Form

It should be remarked that, instead of specifying θ1(s1)and θ2(s2) through (64), it is possible to use the movingcoordinates frame called the Frenet frame defined by the pairof unit vectors (e1(s), n1(s)) expressed by local coordinatesOm-XY . Then, evolution of the frame (e1(s), n1(s)) alongthe curve length must be determined by the Frenet-Serretformula

dds

(e1(s), n1(s)

) = (e1(s), n1(s))( 0 −κ1(s)

κ1(s) 0

), (C.1)


where κ1(s) denotes the curvature of the left-hand objectcontour at arc length s. On the other hand, these two unitvectors are expressed in terms of the inertial frame O-xy bythe formula

n1(θ) = Rθn1(s)

= (rX , rY)

n1(s)

=(

cos θ sin θ− sin θ cos θ

)(cos θ1(s)− sin θ1(s)

)

=(

cos(θ + θ1

)− sin

(θ + θ1

),

),

e1(θ) = Rθe1(s)

=(

sin(θ + θ1

)cos

(θ + θ1

))

,

(C.2)

where Rθ ∈ SO(2) and it is subject to

ddtRθ = Rθ

(0 ωz−ωz 0

), (C.3)

where ωz = θ. Then, it is evident that

ln1(s)n1(θ)− le1(s)e1(θ) =(xm − x1

ym − y1

),

(x1

y1

)=(x01

y01

)+ r1n1(s).

(C.4)

From these equalities, it follows that

ln1(s) = nT1 (θ)

(xm − x01

ym − y01

)− r1,

le1(s) = −eT1 (θ)

(xm − x01

ym − y01

)

= Y1(t),

(C.5)

which show equalities (66) and (68). Hence, (74) thatexpresses translational motion of the object can be writtenin the Frenet form

M

(xy

)− f1n1(θ)− λ1e1(θ)− f2n2(θ)− λ2e2(θ). (C.6)

Note that at the right-hand contact point the Frenet formis defined by (−e2(s), n2(s)), and hence the Frenet-Serretformula is written as

dds

(e2,−n2

) = (e2,−n2)( 0 −κ2(s)

κ2(s) 0

). (C.7)

References

[1] B. Sicilliano and O. Khatib, Eds., Springer Handbook ofRobotics, Springer, New York, NY, USA, 2008.

[2] I. Chavel, Riemannian Geometry: A Modern Introduction,Cambridge University Press, Cambridge, UK, 1993.

[3] M. Sekimoto, S. Arimoto, S. Kawamura, and J.-H. Bae,“Skilled-motion planning of multi-body systems based uponRiemannian distance,” in Proceedings of IEEE InternationalConference on Robotics and Automation (ICRA ’08), pp. 1233–1238, Pasadena, Calif, USA, May 2008.

[4] S. Arimoto, Control Theory of Nonlinear Mechanical Systems:A Passivity-Based and Circuit-Theoretic Approach, OxfordUniversity Press, Oxford, UK, 1996.

[5] J. Jost, Riemannian Geometry and Geometric Analysis,Springer, Berlin, Germany, 2002.

[6] S. Gallot, D. Hulin, and J. Lafontaine, Riemannian Geometry,Springer, Berlin, Germany, 2004.

[7] N. H. McClamroch and D. Wang, “Linear feedback controlof position and contact force for a nonlinear constrainedmechanism,” Journal of Dynamic Systems, Measurement, andControl, vol. 112, no. 4, pp. 640–645, 1990.

[8] D. Wang and N. H. McClamroch, “Position and force con-trol for constrained manipulator motion: Lyapunov’s directmethod,” IEEE Transactions on Robotics and Automation, vol.9, no. 3, pp. 308–313, 1993.

[9] S. Arimoto, H. Hashiguchi, and R. Ozawa, “A simple controlmethod coping with a kinematically ill-posed inverse problemof redundant robots: analysis in case of a handwriting robot,”Asian Journal of Control, vol. 7, no. 2, pp. 112–123, 2005.

[10] A. M. Bloch, M. Reyhanoglu, and N. H. McClamroch, “Con-trol and stabilization of nonholonomic dynamic systems,”IEEE Transactions on Automatic Control, vol. 37, no. 11, pp.1746–1757, 1992.

[11] G. Liu and Z. Li, “A unified geometric approach to modelingand control of constrained mechanical systems,” IEEE Trans-actions on Robotics and Automation, vol. 18, no. 4, pp. 574–587,2002.

[12] S. Arimoto, H. Hashiguchi, M. Sekimoto, and R. Ozawa,“Generation of natural motions for redundant multi-jointsystem: a differential-geometric approach based upon theprinciple of least actions,” Journal of Robotic Systems, vol. 22,no. 11, pp. 583–605, 2005.

[13] S. Arimoto, “A differential-geometric approach for 2D and 3Dobject grasping and manipulation,” Annual Reviews in Control,vol. 31, no. 2, pp. 189–209, 2007.

[14] L. D. Landau and E. M. Lifshitz, Course of Theoretical Physics:Mechanics, vol. 1, Butterworth-Heinemann, Oxford, UK, 3rdedition, 1982.

[15] A. D. Lewis, “Simple mechanical control systems with con-straints,” IEEE Transactions on Automatic Control, vol. 45, no.8, pp. 1420–1436, 2000.

[16] W. M. Oliva, Geometric Mechanics, vol. 1798 of Lecture Notesin Mathematics, Springer, Berlin, Germany, 2002.

International Journal of

AerospaceEngineeringHindawi Publishing Corporationhttp://www.hindawi.com Volume 2010

RoboticsJournal of

Hindawi Publishing Corporationhttp://www.hindawi.com Volume 2014


Active and Passive Electronic Components

Control Scienceand Engineering

Journal of



RotatingMachinery


Hindawi Publishing Corporation http://www.hindawi.com

Journal ofEngineeringVolume 2014

Submit your manuscripts athttp://www.hindawi.com

VLSI Design



Shock and Vibration


Civil EngineeringAdvances in

Acoustics and VibrationAdvances in



Electrical and Computer Engineering

Journal of

Advances inOptoElectronics

Hindawi Publishing Corporation http://www.hindawi.com

Volume 2014

The Scientific World JournalHindawi Publishing Corporation http://www.hindawi.com Volume 2014

SensorsJournal of


Modelling & Simulation in EngineeringHindawi Publishing Corporation http://www.hindawi.com Volume 2014


Chemical EngineeringInternational Journal of Antennas and

Propagation




Navigation and Observation



DistributedSensor Networks


a riemannian-geometry approach for modeling and control of...

Documents