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

Authors: Suguru Arimoto; Morio Yoshida; Masahiro Sekimoto; Kenji Tahara
Journal: Journal of Robotics
 (2009)

---

## Abstract

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 (or nonredundant) robot
under a holonomic constraint is reinterpreted in terms of
“submersion” in Riemannian geometry. A force control
signal constructed in the image space of the constraint gradient
is regarded as a lifting (or pressing) in the direction orthogonal
to the kernel space. By means of the Riemannian distance on the
constraint submanifold, stability of position control under
holonomic constraints is discussed. Second, modeling and control
of two-dimensional object grasping by a pair of multijoint robot
fingers are challenged, when the object is of arbitrary shape. It
is shown that rolling contact constraints induce the Euler
equation of motion, in which constraint forces appear as wrench
vectors affecting the object. The Riemannian metric is introduced
on a constraint submanifold characterized with arclength
parameters. An explicit form of the quotient dynamics is expressed
in the kernel space with accompaniment of a pair of first-order
differential equations concerning the arclength parameters. An
extension of Dirichlet-Lagrange's stability theorem to
redundant systems under constraints is suggested by introducing a
Morse-Lyapunov function.


---

## Body

## 1. Introduction

Among roboticsists, it is
implicitly known that robot motions can be interpreted in terms of orbits on a
high-dimensional torus or trajectories in ann-dimensional
configuration space. Planning of robot motions has
been investigated traditionally on the basis of
kinematics on a configuration space as an n-dim numerical
space Rn [1].This paper first emphasizes a mathematical observation
that, given a robot as a multibody mechanism withn degrees of freedom whose
endpoint is free, the set of all its postures can be regarded as a Riemannian
manifold (M, g) associated with the Riemannian metric g that
constitutes the robot inertia matrix. A geodesic connecting any two postures
can correspond to an orbit expressed on a local coordinate chart and generated
by a solution to the Euler-Lagrange equation of 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
freedom robot, the collection of all the geodesic paths
describes the “law of inertia” for the
manifold. It is also important to note that geodesic paths are invariant under
any choice of local coordinates. This Riemannian geometry viewpoint is extended
in this paper to an important class of multibody dynamics physically
interacting with an object or with environment through holonomic or/and
nonholonomic (but Pfaffian) constraints. Holonomic constraints are defined as a
set of infinitely differentiable functions from a product manifold of multibody
Riemannian manifolds onto an open set of a 2- or 3-dimensional Euclidean space
called the task space. Such a mapping can be treated as a submersion from the
product Riemannian manifold to m(=2-or3-)
dimensional Euclidean space. Hence, holonomic constraints induce a Riemannian
submanifold with a naturally induced metric. An Euler-Lagrange equation is
formulated in an implicit function form under such constraints. It is also
shown that if the gravity term can be explicitly compensated and there arises
no viscous friction then the geodesic motion is invariant, that is, it is
governed by the “law of inertia,” under any adequate lifting (or
pressing) through the joint torque injection in the direction along the
constraint gradient.  An explicit form of the Euler equation whose solution
corresponds to a geodesic on the submanifold is given also as a quotient
dynamics corresponding to the kernel space as an orthogonal compliment to the
image space spanned from all the constraint gradients. Based upon these
observations, the well-known methodology of hybrid (position/force) control for
a robot whose end effector is constrained on a surface is re-examined and shown
to be effective even if the robot is of redundancy in its degrees of freedom.In a latter part of the paper, modeling of dynamics of
grasping and manipulation of a two-dimensional rigid object with arbitrary
shape by using a pair of multijoint robot fingers with spherical finger ends is
challenged. It is shown that rolling contact constraints between finger ends
and the object surfaces induce not only two holonomic constraints but also two
nonholonomic constraints that restrict tangent vectors on the original
Riemannian manifold that is a product of three manifolds expressing a set of
whole postures of the two fingers and the object. An Euler-Lagrange equation
for expressing the dynamics of such physical interaction is derived through
applying the variational principle together with deriving a set of the
first-order differential equations expressing the contact positions of the
object with both the finger ends. The Riemannian distance is introduced on the
kernel space as an orthogonal compliment to the image space of all the
gradients vectors of both contact and rolling constraints. In other words,
rolling constraints are expressed in terms of the first fundamental forms of
given contours of the object and restrict only the tangent vector fields at
both the contact points. An explicit Euler-Lagrange equation corresponding to a
path on the constraint submanifold is derived together with a set of the
first-order differential equations expressed in terms of the second fundamental
forms of the object contours. Thus, it is shown that rolling constraints can be
characterized by means of arc length parameters of the object contours that
express locations of the contact points and in the sequel are integrable in the
sense of Frobenius. A coordinated control signal called “blind grasping”
without referring to the object kinematics or external sensing is proposed and
shown to be effective in realizing stable grasping in the sense of stability on
a submanifold. A sketch of the convergence proof is given on the basis of an
extension of the Dirichlet-Lagrange theorem to a system of degrees of freedom
redundancy by finding a Morse-Lyapunov function and using its physical
properties and mathematical meanings.


## 2. Riemannian Manifold: A Set of All
Postures

Lagrange's equation of motion of a multijoint system
with 2 degrees of freedom (DOF) shown in Figure1
is described by the formula(1)H(q)q¨+{12H˙(q)+S(q,q˙)}q˙+g(q)=u,where q=(q1,q2)T denotes the
vector of joint angles, H(q)  denotesthe
inertia matrix,  S(q,q˙)q˙ the
gyroscopic force term including centrifugal and Coriolis forces, g(q) the
gradient vector of a potential function P(q) due to the
gravity with respect to q, and u the joint
torque generated by joint actuators [4]. It is well known that the inertia matrix H(q) is symmetric
and positive definite, and there exist a positive constant hm together with a
positive definite constant diagonal matrix H0 such
that(2)hmH0≤H(q)≤H0for any q. It should be also noted that S(q,q˙) is skew
symmetric and linear and homogeneous in q˙. More in detail, the ijth entry of S(q,q˙) denoted by sij can be
described in the form [3](3)sij=12{∂∂qj(∑k=1nq˙khik)−∂∂qi(∑k=1nq˙khjk)},where H(q)=(hij(q)), from which it follows apparently that sij=−sji. Since we assume that the objective system to be
controlled is a series of rigid links serially connected through each
rotational joint with single DOF, every entry of H(q) is a constant
or a sinusoidal function of components of joint angle vector q. That is, each element of H(q) and g(q) is
differentiable of class C∞ (infinitely
differentiable in q).Figure 1
A 2-DOF part of
vertically revolute robot PUMA-560.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 set of all such possible postures by M and introduce a
family of subsets of M such that, for
any p∈M with joint
angles p=(θ1,θ2) and any number α>0, a set of all p′=(θ1′,θ2′) is defined
as(4)Up,α={p′:∥p′−p∥H<α},where(5)∥p′−p∥H=∑i,jhij(p)(θi′−θi)(θj′−θj)can be regarded as an open
subset of M. Then, the set M with such a
family of open subsets can be regarded as a topological manifold. It is
possible to show that the manifold M becomes
Hausdorff and compact. Further, every point p of M has a neighborhood U that is
homeomorphic to an open subset Ω of
2-dimensional numerical space R2. Such a 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 with 2
dimensional numerical coordinates (q1,q2) with the origin O (Figure 2). In
this case, it is possible to see that the original set M of robot
postures can be visualized as a torus shown in R3 (see Figure 3)
in which angles q1 and q2 are defined. It
is quite fortunate to see that, in the case of typical robots like the one
shown in Figure 1, the local coordinates (q1,…,qn) can be
identically chosen as a set of n independent
joint angles (θ1,…,θn) by setting qi=θi(i=1,…,n). It is also interesting to see that the torus in
Figure 3 is made to be topologically coincident with the set of all arm
endpoints P=(x,y,z). As discussed in detail in mathematical text books
[5, 6], the topological manifold (M,p) of such a torus
can be regarded as a differentiable manifold of class C∞.Figure 2
A homeomorphismϕ:U→Ω is called a
chart.Figure 3
A
two-dimensional torus is expressed byT2=S1×S1.Now, it is necessary to define a tangent vector to an
abstract differentiable manifoldM at p∈M. Let I be an interval (−ϵ,ϵ) and define a
curve c(t) by a mapping c:I→M such that c(0)=p. A tangent vector to M at p is an
equivalence class of curves c:I→M for the
equivalence relation ~ defined byc~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 to t∈I. This definition of tangent vectors to M at p does not depend
on choice of the coordinate chart at p, as discussed in text books [5, 6]. Let us denote the set of
all tangent vectors to M at p by TpM and call it the
tangent space at p∈M. It has an n-dimensional
linear space structure like Rn. We also denote the disjoint union of the tangent
spaces to M at all the points
of M by TM and call it the
tangent bundle of M.Now, we
are in a position to define a Riemannian metric on 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 definite quadratic form(6)gp(u,v)=∑i,j=1ngij(p)uivj.Suppose thatM is a connected
Riemannian manifold. If c:I[a,b]→M is a curve
segment of class C∞, we define the length of c to
be(7)L(c)=∫ab∥c˙(t)∥dt=∫abgc(t)(c˙(t),c˙(t))dt,where we assume c˙(t)≠0 for any t∈I and call such a
curve 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 exists a finite subdivision a=a0<a1<⋯<ak=b such that c(t) for t∈[ai−1,ai] is a regular
curve for i=1,…,k. Then, it is possible to define for any pair of
points p,p′∈M the Riemannian
distance d(p,p′) to be the
infimum of all admissible curves from p to p′. It is well known [4, 5] that, with the distance function d defined above,
any connected Riemannian manifold becomes a metric space whose induced topology
is coincident with the given manifold topology. An admissible curve c in a Riemannian
manifold is said to be minimizing if L(c)≤L(c˜) for any other
admissible curve c˜ with the same
endpoints. It follows immediately from the definition of distance that c is minimizing
if and only if L(c) is equal to the
distance between its endpoints. Further, it is known that if the Riemannian
manifold {M,g} is complete,
then for any pair of points p and 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 curve c(t) is described
with the aid of coordinate chart (U,ϕ) as ϕ(c(t))=(q1(t),…,qn(t)), then q(t)=ϕ(c(t)) satisfies the
2nd-order differential equation(8)d2dt2qk(t)+∑i,j=1nΓijk(c(t))dqi(t)dtdqj(t)dt=0,(k=1,…,n),where Γijk denotes
Christoffel's symbol defined by(9)Γijk=12∑h=1ngkh(∂gih∂qj+∂gjh∂qi−∂gij∂qh),and (gkh) denotes the
inverse of matrix (gkh). A curve q(t):I→U satisfying (8)
together with ϕ−1(q(t)) is called a
geodesic, and (8) itself is called the Euler-Lagrange equation or the geodesic
equation.Given aC∞-class curve c(t)=I[a,b]→M, the quantity(10)E(c)=12∫ab∥c˙(t)∥2dt=12∫abgc(t)(c˙(t),c˙(t))dtis called the energy of the
curve. Then, by applying the Cauchy-Schwartz inequality for (7), we
have(11)L(c)2≤2(b−a)E(c).Further, the equality of (11)
follows if and only if ∥c˙(t)∥ is constant. It
is also possible to see that if c(t) is a geodesic
with c(a)=p and c(b)=p′, then for any other curve c¯(t) with the same
endpoints, it holds(12)E(c)=L(c)22(b−a)≤L(c¯)22(b−a)≤E(c¯).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)=p and c(b)=p′. In mechanics, E(c) is usually
called “action of c,” and c(t) is considered
as the orbit of motion of a multibody system.


## 3. Riemannian Geometry of Robot Dynamics

Dynamics of a robotic mechanism withn rigid bodies
connected in series through rotational joints are described by Lagrange's
equation of motion, as shown in (1). It is implicitly assumed that the axis of
rotation of the first body is fixed in an inertial frame and denoted by z-axis that is
perpendicular to the xy-plane as shown
in Figure 1. If there is no gravity force affecting motion of the robot, then
the equation of motion of the robot can be described by the
form(13)H(q)q¨+{12H˙(q)+S(q,q˙)}q˙=u,where u stands for a
vector of control torques emanating from joint actuators. This formula is valid
for motions of a revolute joint robot, shown in Figure 1, if it is installed in
weightless environment like an artificial satellite, or the gravity term g(q) (included in
(1)) can be compensated by joint actuators through control input u. In general, we can represent a posture p of the robot as
a physical entity by a family of joint angles θi(i=1,…,n), which can be expressed by a point Θ=(θ1,…,θn) in the n-dimensional
numerical space Rn. In fact, we can naturally imagine an isomorphism ϕ:U→Ω, where U⊂M, and Ω is an open
subset of Rn. In other words, a local coordinate chart ϕ(U)(=Ω) can be treated
to be identical to U itself, an open
subset of M, by regarding q=(q1,…,qn)T (“T” denotes transpose and hence q a column
vector) identical to Θ by setting qi=θi(i=1,…,n). In this way, the abstract manifold M as the set of
all robot postures can be regarded as an n-dimensional
torus Tn as an n-tuple direct
product of S1:Tn=S1×⋯×S1. Hence, a robot posture p∈M can be represented
by a point Θ on Tn and also
expressed by a joint vector q in Rn.From the definition of inertia matrices,H(q) is symmetric
and positive definite, and the kinetic energy is expressed as a quadratic
form(14)K(q,q˙)=12q˙TH(q)q˙.Hence, the equation of motion of
the robot is expressed by Lagrange's equation(15)ddt{∂∂q˙L(q,q˙)}−∂∂qL(q,q˙)=u,where L(q,q˙)=K(q,q˙), and u stands for a
generalized external force vector. It is interesting to note that in
differential geometry, (15) can be described as(16)∑ihkiq¨i+∑i,jΓikj(q)q˙iq˙j=uk,where Γikj denotes
Christoffel's symbol of the first kind defined by(17)Γikj=12(∂hjk∂qi+∂hik∂qj−∂hij∂qk).For later use, we introduce
another Christoffel's symbol called the second kind as shown in the
formula(18)Γijk=12∑l=1nhlk(∂hjl∂qi+∂hil∂qj−∂hij∂ql)=12∑l=1nhlkΓilj,where (hlk) denotes the
inverse of (hlk), the inertia matrix H(q)=(hlk). Since (hkl) and (hkl) are symmetric,
it follows that Γikj=Γjki and Γijk=Γjik. Now, we show that (13) is equivalent to (16) by
bearing in mind that H˙(q)=∑i{∂H(q)/∂qi}q˙i, and the skew symmetric matrix S(q,q˙) is expressed as
in (3). In fact, the second term in the bracket () 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(19)∑j=1nskjq˙j=∑j=1n12[{∂∂qj(∑i=1nq˙ihki)}q˙i−{∂∂qk(∑i=1nq˙ihij)}q˙j]=∑j=1n∑i=1n12{(∂hik∂qj−∂hij∂qk)}q˙iq˙j.Substituting this into (16) by
comparing the last two terms of (17) with the last bracket {} of (19) results in the equivalence of (13)
to (16). It is easy to see that multiplication of (16) by H−1(q) yields(20)q¨k+∑i,jΓijkq˙iq˙j=∑jhkjuj,k=1,…,n.If u=0, this expression is nothing, but the Euler-Lagrange
equation shown in (8). By this reason, from now on, we use symbol gij(q) instead of hij(q) for the inertia
matrix H(q) even when robot
dynamics are treated.Now, on the abstract topological manifoldM as a set of all
possible postures of a robot, suppose that a Riemannian metric is given by a scalar
product on each tangent space TpM:(21)〈v,w〉=gij(p)viwj,where v=vi(∂/∂qi)∈TpM and w=wj(∂/∂qj)∈TpM, and the summation symbol ∑ in i and j is omitted, and q=(q1,…,qn) represents
local coordinates. Then, the manifold (M,p) can be regarded
as a Riemannian manifold and becomes complete as a metric space. Then,
according to the Hopf-Rinow theorem [5], any two points p,q∈M can be joined
by a geodesic of length d(p,q), that is, a curve satisfying (8) with shortest
length, where(22)d(p,q)=inf∫ab∥c˙(t)∥dt=inf∫ab〈c˙(t),c˙(t)〉dtwith c(a)=p and c(b)=q.As discussed in the previous section, geodesics are
the critical points of the energy functionalE(c). Further, a geodesic curve c(t) satisfies ∥c˙(t)∥=const. In fact, by
regarding c(t)=q(t) that is an
orbit on Ω, we have(23)ddt〈q˙,q˙〉=ddt(gij(q(t))q˙i(t)q˙j(t))=ddt{q˙T(t)G(q(t))q˙(t)}=q¨T(t)G(q)q˙(t)+q˙T(t)G˙(q)q˙(t)+q˙T(t)G(q)q¨(t)=0,where the equivalent
expression(24)G(q)q¨+{12G˙(q)+S(q,q˙)}q˙=0to (13) with u=0 is used, G(q)=(gij(q)), and sij of S is given in the
form of (3) (where hij=gij). It is also
important 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 solution to (20) (where uj is of C∞ in t) should
satisfy(25)∑j=1nq˙j(t)uj(t)=ddtE(q(t)),or(26)∫ab∑j=1nq˙j(t)uj(t)dt=E(q(b))−E(q(a)),as long as q(t)∈Ω, where E(q(t))=(1/2)〈q˙(t),q˙(t)〉. When u(t)=0, E(q(t))=const. and then the
curve connecting p=ϕ−1(q(a)) and p′=ϕ−1(q(b)) must be a
geodesic. In other words, an inertia-originated movement without being affected
by the gravitational field or any external force field produces a geodesic
orbit [2]. The most
importantly, geodesics together with their length
are invariant under any choice of local coordinates.Before closing this expository section on robot motion
from the Riemannian geometry viewpoint, we must emphasize that all the above
invariant properties of geodesics of inertia-originated robot motions result
from imaging a set of all robot postures as an abstract Riemannian manifold.
Choice of a local coordinates is originally arbitrary. Even ann-dimensional
torus Tn is one of such
choice of local coordinates 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
tangent space TpM is
indeterminable or free to choose. However, once a local coordinates system for
an n  degrees of freedom
robot is chosen by joint angle vector q=(q1,…,qn)T, then the coordinates in the tangent space TpM should be
chosen as the vector of joint angular velocities ∂/∂q=(∂/∂q1,…,∂/∂qn) correspondingly
to q˙=(q˙1,…,q˙n)T, from which the Riemannian metric gij(q) is defined
through the inertia matrix.


## 4. Constraint Submanifold and
Hybrid Position/Force Control

Consider ann-DOF robotic
arm whose last link is a pencil and suppose that the endpoint of the pencil is
in contact with a flat surface φ(x)=ξ, where x=(x,y,z)T. It is well known that the Lagrange equation of
motion of the system is written as(27)G(q)q¨+{12G˙+S}q˙+g(q)=−λ∂φ(x(q))∂q+u,where ∂φ/∂q can be
decomposed into ∂φ(q)/∂q=JT(q)∂φ/∂x and J(q)=∂x/∂qT. On the constraint manifold Fξ={p:p∈Mandφ(x(p))=ξ}, let us consider a smooth curve c(t):I[a,b]→Fξ that connects
the given two points c(a)=p and c(b)=p′, where p and p′ belong to Fξ. The length of such a curve constrained to Fξ is defined
as(28)L(c)=∫abgij(c(t))c˙i(t)c˙j(t)dt,and consider the
minimization(29)d(p,p′)=infc∈FξL(c)that should be called the
distance between p and p′ on the
constraint manifold. Then, the minimizing curve called the geodesic denoted
identically by q(t)(=c(t)) must satisfy
the Euler equation(30)q¨k(t)+Γijkq˙i(t)q˙j(t)=−λ(t)⋅(gradφ(x(t)))k,together with the constraint
condition φ(x(t))=ξ, where(31)gradφ(x(t))=G−1(q(t))JT(q)∂φ∂x,and JT(q)=∂xT/∂q. It should be noted that,
from the inner product of (30) and w=JT∂φ/∂x, it follows
that(32)∑k(wkq¨k+wkΓijkq˙iq˙j)=−λ∑kwkgkjwj.Since the holonomic constraint φ(x(q))=ξ implies 〈w,q˙〉=0, it follows that(33)∑k=14(wkq¨k+(ddtwk)q˙k)=0.Substituting this into (32), we
obtain(34)λ(t)=1wTG−1w{∑k(w˙kq˙k−∑i,jwkΓijkq˙iq˙j)}.From the Riemannian geometry,
the constraint force λ(t) with the grad{φ(x(q))} stands for a
component of the image space of w(=JT(q)∂φ/∂x) that is
orthogonal to the kernel TFξ of w. In other words, this component is cancelled out by
the image space component of the left hand side of (32). From the physical
point of view, λ(t) should be
regarded as a magnitude of the constraint force that presses the surface φ(x(q))=ξ in its normal
direction. In order to compromise the mathematical argument with such physical
reality, let us suppose that the joint actuators can supply the control
torques(35)u=λd⋅JT(q)∂φ∂x+g(q).Then, by substituting this into
(27), we obtain the Lagrange equation of motion under the constraint φ(x(q))=ξ:(36)G(q)q¨+{12G˙+S}q˙=−(λ−λd)JT(q)∂φ∂x=−ΔλJT(q)∂φ∂x,where Δλ=λ−λd. It should be noted that introduction of the first
term of control signal of (35) does not affect the solution orbit on the
constraint manifold, and further it keeps the constraint condition during
motion by rendering λ(t)(=λd+Δλ(t)) positive. In a
mathematical sense, exertion of the joint torque λdJT(∂φ/x) plays a role of
“pressing” or “lifting” of the image space spanned from the
gradient of the constraint equation. Further, note that (36) is of an implicit
function form with the Lagrange multiplier Δλ. To affirm the argument of treatment of the geodesics
through this implicit form, we show an explicit form of the Lagrange equation
expressed on the orthogonally projected space (kernel space) by introducing the
orthogonal transformation(37)q˙=(P,w∥w∥−1)(η˙z˙)=Qq¯˙,where P is a 4×3 matrix whose
column vectors with the unit norm 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 attained
by multiplying (36) by PT from the left
such that(38)PTG(q)ddt(Pη˙)+PT{12G˙+S}Pη˙=0,which is reduced to the Eular
equation in η˙:(39)G¯(q)η¨+{12G¯˙+S¯}η˙=0,or equivalently(40)η¨k+Γ¯ijkη˙iη˙j=0,k=1,…,n−1(=3),where G¯(q)=PTG(q)P, Γ¯ijk the Christoffel
symbol for (g¯ij)=G¯, and(41)S¯=PTSP−12P˙TGP+12PTGP˙,which is skew symmetric, too.
Note that the transformation Q is isometric,
and (40) stands for the geodesic equation on the constraint Riemannian
submanifold.


## 5. Hybrid Control for Redundant Systems
and Stability on a Submanifold

Let us now reconsider a
hybrid position/force feedback control scheme,
which is of the form(42)u=g(q)+λdJT(q)∂φ∂x+Cq˙+JT(q)(ζkx˙+kΔx),where φ(x)=z, x˙=(x˙,y˙,0)T, Δx=(x−xd,y−yd,0). This type of hybrid control is an extension of the
McClamroch and Wang's method [7, 8] to cope with a robotic
system that is subject to redundancy in DOFs. From now on, we redefine the
Jacobian matrix J(q) as 2×4 matrix J(q)=(∂x/∂qT,∂y/∂qT) because we
consider a special 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 that the orthogonality
relationship among (x,y,z) coordinates in E3 does not imply
the orthogonality among ∂x/∂q, ∂y/∂q, and ∂z/∂q(=∂φ/∂q). Therefore, ∂z/∂q is not
orthogonal to JT(q)(=(∂x/∂q,∂y/∂q)), and hence the last
term of the right hand side of (42) contains both components of image (w) and ker(w).Now, substituting this into (27)
yields(43)G(q)q¨+{12G˙+S+C}q˙+JT(q){ζkx˙+kΔx}=−Δλ∂z(q)∂q.Evidently the inner product of
(43) and q˙ under the
constraint z(q)=ξ leads
to(44)ddtE(q,q˙)=−q˙Cq˙−ζk∥x˙∥2,where(45)E(q,q˙)=12q˙TG(q)q˙+k2∥Δx∥2.Unfortunately, this quantity is
not positive definite in the tangent bundle TFξ. Nevertheless, it is possible to see that magnitudes
of q˙(t) and Δx(t) remain small if
at initial time t=0 both magnitudes ∥q˙(0)∥ and ∥Δx(0)∥ are set small.
Next, let us introduce the equilibrium manifold defined by the
set(46)EM1={q:z(q)=ξ,x(q)=xd,y(q)=yd},which is of one dimension.
Fortunately as discussed in the paper [9], it is possible to confirm that a modified scalar
function(47)Wα=E(q,q˙)+αq˙TG(q)PφJT(q)Δxbecomes positive definite in TFξ with an
appropriate positive parameter α>0, provided that the physical scale of the robot is
ordinary, and feedback gains C and k are chosen
adequately. Here, in (47), Pφ is defined as I4−wwT/∥w∥2. Furthermore, if in a Riemannian ball in Fξ defined as B(q(0),r0)={q:d(q,q(0))<r0}, the Jacobian
matrix J(q) is
nondegenerate, then it can be expected that there exist positive numbers σm and σM such
that(48)σMI2≥JJT≥JPφJT≥σmI2.Further, to simplify the
argument, we choose C=cI4 with a constant c>0. Then, it follows from differentiation of V(=q˙TG(q)PφJT(q)Δx) that(49)V˙=−{ζkx˙+kΔx}TJPφJTΔx−cq˙TPφJTΔx+q˙TGPφJTx˙−h(q˙,G)Δx,where(50)h(q˙,G)=q˙T{(12G˙Pφ−SPφ−GP˙φ)JT−GPφJ˙T}.This 1×2-vector h is quadratic in
components of q˙, and each
coefficient of q˙iq˙j (for i,j=1,…,4) is at most of
order of the maximum spectre radius of G(q) denoted by gM. Hence, it follows from (49) that(51)|h(q˙,G)Δx|≤gMl0∥q˙∥2∥Δx∥,where l0 signifies a
constant that is of order of the maximum link length (because the Jacobian
matrix ∂x/∂qT is
homogeneously related to the three link lengths of the robot shown in Figure 4). Further, note that q˙TPφ=0 in this case
and remark that(52)q˙TGPφJTx˙≤12q˙TGGq˙+12x˙TJPφJTx˙,(53)−ζkx˙TJPφJTΔx≤ζk2ΔxTJPφJTΔx+ζ2x˙T(JJT)x˙.Thus, substituting (51) to (52)
into (49) yields(54)V˙≤−{k−12−ζk2}ΔxTJPφJTΔx+12q˙TGGq˙+1+ζ2x˙TJJTx˙+gMl0∥q˙∥2∥Δx∥.With the aid of a positive
parameter α>0, it is easy to see that(55)Wα≥E−α2q˙TG(q)G(q)q˙−α2ΔxTJPφJTΔx=12q˙T{G−αGG}q˙+12ΔxT(kI2−αJPφJT)Δx.Figure 4
A hand-writing
robot with four DOFs whose endpointP(=(x,y)) is constrained
on a plane z=ξ.Now, suppose that the robot has an ordinary physical
scale with(56)gM≤0.001[kgm2],l0≤0.2[m].Then, it is possible to see
that(57)W˙α=E˙+αV˙≤−αkσm2∥Δx∥2−c3∥q˙∥2,provided that ∥Δx(t)∥<0.3[m], (1+ζk)≤k, and ζk>α(1+ζ)/2. From the practical point of view of control design
for the handwriting robot, ζ is set around ζ=2/2, and k is chosen
between 5.0~20.0[kg/s2] (see
[9]). On account of
(48) and (56),(58)Wα≥(1−αγ0)E,and similarly,(59)Wα≤(1+αγ0)E,where γ0=max(gM,σM/k). Then, if the damping factor can be selected to
satisfy both inequalities c≥(3/2)(ασmgM) and c>αgMl0, then (57) is
reduced to(60)W˙α≤−ασm1+αγ0Wα,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(61)Wα(t)=Wα(0)e−γt,where γ=kσm/3σM.Now, suppose thatq* in EM1∩B(q(0),r0) is the
minimizing point that connects with q(0) among all
geodesics from q(0) to any point of EM1∩B(q(0),r0). We call q* a reference
point corresponding to q(0).Definition 1 (stable Riemannian ball on a submanifold).
If for anyε>0, there exists
the number δ(ε)>0 such that any
solution trajectory (orbit) of (43) starting from an arbitrary initial position
inside B(q*,δ(ε)) with q˙(0)=0 remains inside B(q*,ε) for any t>0, then the reference point q* on EM1 is said to be
stable on a submanifold (see Figure 5).
It can be concluded from the exponential convergence ofWα 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 further such a solution trajectory converges asymptotically to
some q∞ on the
equilibrium manifold EM1 in an
exponential speed of convergence. This can be well understood as a natural
extension of the well-known Dirichlet-Lagrange stability under holonomic
constraints to a system with DOF-redundancy. The details of the proof are
presented in Appendix .Figure 5
Definition of
the Riemannian ballB(q*,δ). Any orbit starting from B(q*,δ) converges to EM1∩B(q*,ε).It should be noticed from the proof in Appendix that
asymptotic convergence of the solution trajectory to some q∞ on the equilibrium
manifold implies also the asymptotic convergence of constraint force λ(t) to λd as t→∞ because Δλ=λ−λd is expressed
as(62)Δλ(t)=1wTG−1w{∑k(w˙q˙k−∑i,jwkΓijkq˙iq˙j)−wTJT(ζkx˙+kΔx)},and this right hand side
converges to zero as t→∞.The stability notion of a Riemannian ball in a
neighborhood of a reference equilibrium stateq* on EM1 is extended to
cope with the case that the initial velocity vector q˙(0) is not zero. To
do this, we define an extended Riemannian ball in the tangent bundle M×TFξ around (q*,q˙=0) in such a way
that(63)B{(q*,0);(r0,rK)}={(q,q˙):d(q,q*)<r0,(1/2)q˙TG¯(q)q˙<rK},where d(q,q*) denotes the
distance between q and q* restricted to
the submanifold Fξ, and G¯ is defined
below (40).Definition 2 (asymptotic stability on a submanifold).
If for anyε>0, there exist
numbers δ(ε) and δK(ε) such that any
solution trajectory of (43) starting from an arbitrary initial position and
velocity inside B{(q*,0);(δ(ε),δK(ε))} remains in B{(q*,0);(ε,rK)} and further
converges asymptotically to some equilibrium point q∞∈EM1 with still
state, then the reference point with its posture on EM1 is said to be
asymptotically stable on a constraint submanifold.It should be remarked that Bloch et al. [10] introduces originally the concept of stabilization for a class of
nonholonomic dynamic systems based upon a certain configuration space. The
redefinition of stability concepts introduced above is free from any choice of
configuration spaces (local coordinates) and assumptions on an invertibility
condition (that is almost equivalent to nonlinear control based on compensation
for nonlinear inertia-originated terms). Liu and Li [11] also gave a geometric
approach to modeling of constrained mechanical systems based upon orthogonal
projection maps without deriving a compact explicit form of the Euler equation
like (40) with a reduced dimension due to constraints. Therefore, the proposed
control scheme was developed on the basis of compensation for the inertia-originated
nonlinear terms (that is almost equivalent to the computed torque method). A
naive idea of stability on a manifold by using different metrics for the
constrained submanifold and its tangent space was first presented in [9] and used in stabilization
control of robotic systems with DOF redundancy [12, 13].


## 6. 2-dimensional Stable Grasp of a Rigid
Object with Arbitrary Shape

Consider a control problem for stable grasping of a 2D
rigid object by a pair of planar multijoint robot fingers with hemispherical
fingertips as shown in Figure6. In this figure, the two robots are installed
on the horizontal xy-plane E2. We denote the object mass center by Om with the
coordinates (xm,ym) expressed in
the inertial frame. On the other hand, we express a local coordinate system
fixed at the object by Om-XY together with
unit vectors rX and rY along the X-axis and Y-axis,
respectively (see Figure 7). The left-hand side surface of the object is
expressed by a curve c(s) with local
coordinates (X(s),Y(s)) in terms of arc
length parameter s as shown in
Figure 7.Figure 6
Two-dimensional
object grasping by a pair of multijoint robot fingers.Figure 7
Definitions of
physical symbols in relation to rolling contact.θ1 denotes θ1(s) appearing in
(64).First, suppose that at the left-hand contact pointP1 the fingertip
of the left finger is contacting with the object. Denote the unit normal at P1 by n1 and the unit
tangent vector by e1. Note that n1 is normal to
both the object surface and 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 a function on the curve:(64)θ1(s)=arct(X′(s)Y′(s)),where X′(s)=dX(s)/ds and Y′=dY(s)/ds. In this paper, all angles are set positive in
counterclockwise direction. Then,(65)P1P1′¯=ln1(s)=−X(s)cosθ1(s)+Y(s)sinθ1(s),which is dependent only on s and, therefore,
a shape function of the object. On the other hand, this length can be expressed
by using the inertial frame coordinates in the following way (see Figure
8):(66)P1P1′¯=(xm−x01)cos(θ+θ1(s))−(ym−y01)sin(θ+θ1(s))−r1.Hence, the left-hand contact
constraint can be expressed by the holonomic constraint(67)Q1=−(xm−x01)cos(θ+θ1)+(ym−y01)sin(θ+θ1)+(r1+ln1(s))=0,where ln1(s) denotes the
right-hand side of (65) and θ1=θ1(s) for
abbreviation.Figure 8
Physical and
geometrical meanings of key variablesln1(s) and le1(s) defined in
relation to the object contour c(s) expressed in
terms of length parameter s of the
curve.Next, we note that the lengthOmP1′¯ can be regarded
also as a shape function of the object given by(68)OmP1′¯=le1(s)=X(s)sinθ1+Y(s)cosθ1.On the other hand, this quantity
can be also expressed as(69)OmP1′¯=Y1(t)=−(xm−x01)sin(θ+θ1)−(ym−y01)cos(θ+θ1).As discussed in [13], in the light of the book
[14], pure rolling at P1 is defined by
the condition that the translational velocity of P1 on the finger
sphere is equal to that of P1 on the object
contour along the e1-axis. Hence,
by denoting p1=q11+q12+q13, we have(70)−r1p˙1+r1θ˙=−Y˙1=(x˙m−x˙01)sin(θ+θ1)+(y˙m−y˙01)cos(θ+θ1)+{(xm−x01)cos(θ+θ1)−(ym−y01)sin(θ+θ1)}θ˙,which from (67) can be reduced
to(71)−r1p˙1−ln1θ˙+(x˙01−x˙)sin(θ+θ1)+(y˙01−y˙)cos(θ+θ1)=0.This constraint form is
Pfaffian. As to the contact point P2 at the
right-hand finger end, a similar nonholonomic constraint can be obtained. Thus,
by introducing Lagrange's multipliers f1 and f2 associated with
holonomic constraints Qi=0(i=1,2), it is possible to construct a Lagrangian:(72)L=12∑q˙iTGi(qi)q˙i+12{Mx˙2+My˙2+Iθ˙2}−f1Q1−f2Q2,where qi denote the
joint vector for finger i, Gi(qi) the inertia
matrix for finger i, M and I denote the mass
and inertia moment of the object. Since both the rolling constraints are
Pfaffian, it is possible to associate (71) and its corresponding form at P2 with another
multipliers λi(i=1,2) and regard them
as external forces. Thus, by applying the variational principle to the
Lagrangian together with the external forces, we obtain the Lagrange equation
of motion of the overall fingers-object system:(73)Iθ¨−f1Y1+f2Y2−λ1ln1+λ2ln2=0,(74)M(x¨y¨)−f1n1−f2n2−λ1e1−λ2e2=0,(75)Gi(qi)q¨i+{12G˙i+Si}q˙i+fiJiT(qi)ni+λi{JiT(qi)ei+(−1)iripi}=ui,i=1,2, where p1=(1,1)T, p2=(1,1,1)T, and JiT(qi)=∂(x0i/y0i)/∂qi, and(76)ni=(−1)i(−cos(θ+θi)sin(θ+θi)),ei=(sin(θ+θi)cos(θ+θi))for i=1,2. It is important to note that the object dynamics of
(73) and (74) can be recast in the form(77)H0z¨+f1w1+f2w2+λ1w3+λ2w4=0,where z=(x,y,θ), and(78)wi=(−ni(−1)iYi),wi+2=(−ei(−1)ilni),i=1,2,which are of a two-dimensional
wrench vector. This implies that if the sum of all wrench vectors converges to
zero, then the force/torque balance is established. Further, if we
define(79)X=(x,y,θ,q1T,q2T)T,u=(0,0,0,u1T,u2T)T,G(X)=diag(M,M,I,G1(q1),G2(q2)),S(X,X˙)=diag(0,0,0,S1,S2),then (73) ~ (75) can be
written in the form(80)G(X)X¨+{12G˙+S}X˙+∑i=1,2(fi∂Qi∂X+λi∂Ri∂X)=u,where(81)Ri=−(−1)iri{(θ+θi(si))−piTqi}+Yi+ψi(si),and ψi signifies a function
depending only on the shape parameter si. The details of the integral form of (81) will be
discussed in Appendix . Comparing (80) with (27), it must be understood that a
similar but extended argument of Section 4 can be applied even for a case of
physical interaction with complex holonomic constraints and nonholonomic (but
Pfaffian) constraints. More precisely, when u=0 in (80), the
image space of the Riemannian manifold (X,G(X)) with
constraints Q1=0 and Q2=0 is spanned from
the gradients(82)G−1(X)(∂Q1∂X,∂Q2∂X,∂R1∂X,∂R2∂X),and the kernel space should be
defined as the orthogonal compliment to the image space. Note that the rolling
constraints of (71) and another for i=2 induce further
restriction of the tangent space to a more subdimensional linear space. Here, ∂Ri/∂X should be taken
at the time instant t under the
condition that si are fixed.
Nevertheless, as shown in Appendix , it is quite interesting to know that Ri for i=1,2 can be regarded
as a constant function in change of t even under any
infinitesimally small variation of si, that is, 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 tangent space to an image space of constraint gradients
and its orthogonal compliment is known in general mathematical terminology (see
[15, 16]) but has not yet been
critically spelled out in the case of existence of rolling contact connections
between curved rigid bodies.The Euler equation of (80) can be expressed in a
similar form to (1) by introducing the constraint vector Φ and the vector
of Lagrange multipliers λ as(83)Φ=(Q1,R1,Q2,R2),λ=(f1,λ1,f2,λ2)T.Then, (80) can be written in the
form(84)G(X)X¨+{12G˙+S}X˙+∂Φ∂Xλ=u,which must be valid for the
constraint X˙T(∂Φ/∂X)=0. We denote the (n−4)-dimensional
kernel space of ∂Φ/∂X by VX and its
4-dimensional orthogonal compliment as the image space of G−1(X)∂Φ/∂X by HX.We are now in a position to derive an update law of
the length parameterss1 and s2 along the
object contours. Firstly, it is important to verify that the holonomic
constraint Q1=0 is invariant
under any infinitesimally small variation of s1. In fact, we see from (64) to (69)
that(85)∂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,where we denote s1=s for simplicity.
Next, it is possible to show 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 the object. Then, it is obvious from
Figures 6 to 8 that(86)r1(−Δp1+Δθ+Δθ1)Δt=−Δs=−κ1−1(s)Δθ1Δt,where κ1(s)≠0. Since θ1′(s)=κ1(s), (86) is reduced to(87)r1(−Δp1+Δθ)Δt=−(1+r1κ1(s))ΔsΔt.This equation should be
accompanied with the Euler-Lagrange equation (80). This can be also regarded as
a nonholonomic constraint to (80). As to the right-hand contour, it follows that(88)ds2dt=−r21+r2κ2(s2)(−p˙2+θ˙).


## 7. Lifting in Horizontal Space and
Force/Torque Balance

First, in order to find an adequate lifting that
belongs to the image spaceHX and realizes
the force/torque balance (see (77)) in the sense that(89)f1dw1+f2dw2+λ1dw3+λ2dw4=0, we remark that(90)(xm−x01ym−y01)=Rθ+θ1(r1+ln1(s1)−Y1)(91)−(xm−x02ym−y02)=Rθ+θ2(ln(s2)+r2Y2),where(92)Rθ+θ1=(n1(θ),e1(θ))=(cos(θ+θ1)sin(θ+θ1)−sin(θ+θ1)cos(θ+θ2)).We also define(93)Rθ+θ2=(cos(θ+θ2)sin(θ+θ2)−sin(θ+θ2)cos(θ+θ2)),Rθ=(cosθsinθ−sinθcosθ).Then, it follows from (90) and
(91) that(94)−(x01−x02y01−y02)=Rθ{Rθ1(r1+ln1(s1)−Y1)+Rθ2(r2+ln2(s2)Y2)}=Rθ(l(s,Y1,Y2)−d(s,Y1,Y2)),where s=(s1,s2) and(95)l(s,Y1,Y2)=(r1+ln1)cosθ1+(r2+ln2)cosθ2−Y1sinθ1+Y2sinθ2,d(s,Y1,Y2)=(r1+ln1)sinθ1+(r2+ln2)sinθ2+Y1cosθ1−Y2cosθ2.Thus, let us define(96)fid=fdr1+r2{lcosθi+dsinθi},λid=(−1)ifdr1+r2{−lsinθi+dcosθi},i=1,2,and remark that they satisfy(97)f1dn1+f2dn2+λ1de1+λ2de2=0.Further, note that(98)f1dw1+f2dw2+λ1dw3+λ2dw4=(0,0,SN)T,where(99)SN=fdr1+r2{−(lcosθ1+dsinθ1)Y1+(lcosθ2+dsinθ2)Y2−(lsinθ1−dcosθ1)ln1−(lsinθ2−dcosθ2)ln2}=fdr1+r2{l(r1sinθ1+r2sinθ2)−d(r1cosθ1+r2cosθ2)}.This shows according to (98)
that if SN tends to
vanish, then the force/torque balance is established, that is, the total sum of
wrench vectors exerted to the object becomes zero.


## 8. Control Signal for Blind Grasping and
a Morse-Lyapunov Function

From the practical standpoint of designing a control
signal for stable grasping, it is important to see that objects to be grasped
are changeable, but the pair of robot fingers is
always the same. That is, for designing control signals, we are unable to use
physical parameters of the object such as the location(xm,ym) of its mass
center and object geometry. On the contrary, it is possible to assume the
knowledge of finger kinematics like finger link lengths and locations of the
centers of finger-end spheres and to use measurement data of finger joint
angles and angular velocities. In view of these points, let us propose a family
of control signals defined as(100)ui=−ciq˙i+(−1)ifdr1+r2JiT(qi)(x01−x02y01−y02)−riN^ipi,i=1,2,where(101)N^i(t)=γi−1ripiT(qi(t)−qi(0)),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 overall
fingers-object system can be derived by substitutingui 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:(102)12{(x01−x02)2+(y01−y02)2}=12(l2+d2)fid∂Qi∂qi+λid∂Ri∂qi=(−1)i[JiT{−fid(cos(θ+θi)−sin(θ+θi))+λid(sin(θ+θi)cos(θ+θi))}+λidripi]=(−1)ifdr1+r2{−JiTRθ(l−d)+λidripi}=(−1)ifdr1+r2{JiT(x01−x02y01−y02)+λidripi},i=1,2,Then, by substituting (98),
(102) into (73) to (75), it is possible to express the closed loop
dynamics in the following forms:(103)Iθ¨−Δf1Y1+Δf2Y2−Δλ1ln1+Δλ2ln2−SN=0,M(x¨y¨)+Δf1n1(θ)+Δf2n2(θ)−Δλ1e1(θ)−Δλ2e2(θ)=0,Giq¨i+{12G˙i+Si}q˙i+ciq˙i+ΔfiJiTni(θ)+Δλi{JiTei(θ)+(−1)iripi}+ri{(−1)iλid+N^i}pi=0,i=1,2.  Similarly, to the form of (84),
these equations can be written in the following way:(104)G(X)X¨+{12G˙+S+C}X˙+∂Φ∂XΔλ+SNbθ+∑i=1,2{N^i+(−1)iλid}bi=0,where(105)Δλ=(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.At this stage, it is important
to note that in accordance with four constraints Φ=0, the velocity vector X˙ belongs to the
kernel space of (∂Φ/∂X)T and, therefore, X˙T∂Φ/∂X=0. Further by using (100) and taking inner product of
(84) and X˙, we obtain(106)ddt{K+fd2(r1+r2){(x01−x02)2+(y01−y02)2}+∑i=1,2γi2N^i2}=−∑i=1,2ci∥q˙i∥2,where K denotes the
system's kinetic energy defined by(107)K=∑i=1,212q˙iTGi(qi)q˙i+M2(x˙2+y˙2)+I2θ˙2.The relation of (106) must be
equivalently derived by taking inner product of X˙ and the closed
loop dynamics of equation (104). To verify this, let us define(108)p1=q11+q12+q13=p1Tq1,p2=q21+q22=p2Tq2,(109)P=fd2(r1+r2){l2(s,Y1,Y2)+d2(s,Y1,Y2)}+∑i=1,2γi2N^i2,where l and s are defined in
(95). It should be remarked that, with the aid of expressions of integral form
of rolling constraints shown in (71), we have(110)∂l∂θ=−∂Y1∂θsinθ1+∂Y2∂θsinθ2=−r1sinθ1−r2sinθ2,∂d∂θ=r1cosθ1+r2cosθ2,and hence,(111)∂P∂θ=fdr1+r2{l∂l∂θ+d∂d∂θ}=−fdr1+r2{l(r1sinθ1+r2sinθ2)−d(r1cosθ1+r2cosθ2)}=SN.Similarly, from (66), (69), and
(101), we have(112)∂P∂pi=fdr1+r2{l∂l∂pi+d∂d∂pi}+γiN^i∂N^i∂pi=(−1)ifdr1+r2{l∂Yi∂pisinθi−d∂Yi∂picosθi}+γiN^i∂N^i∂pi=−rifdr1+r2{lsinθi−dcosθi}+riN^i=(−1)iriλid+riN^i,i=1,2.Thus, the inner product of X˙ and (104) or
the relation of (106) is reduced to(113)ddt{K+P}=−∑i=1,2ci∥q˙i∥2.In other words, the closed loop
dynamics of (104) can be expressed in the form(114)G(X)X¨+{12G˙+S+C}X˙+∂P∂X+∂Φ∂XΔλ=0.This is interpreted as a
Lagrange equation of the Lagrangian(115)L=K−P−ΦΔλ,in accompany with the external
damping torques ciq˙i 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 since Yi can be regarded
as a linear function of θ and pi for i=1,2 because of
(71). Hence, P can be regarded
as a Morse function defined on the Riemannian submanifold induced by two
constraints described by Qi=0 for i=1,2 and four
constraints in the tangent space described by Q˙i and R˙i=0 for i=1,2.


## 9. Physical Insights into Gradient and
Hessian of the Morse Function

The physical meaning of control signals for blind
grasping defined by (100) is quite simple. The first term of the right-hand side
of (100) plays a role of damping for rotational motion of finger joints. Damping
for motion of the object is exerted from velocity constraints Q˙i=0 and R˙i=0 for i=1,2 as discussed in
detail in the previous paper [1]. The second term plays a role of fingers-thumb
opposition that induces 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 rotation of finger joints. These characteristics of the control
signal condense into the Lagrange equation of motion with (1) the potential P(s,θ,p1,p2) of (109), (2)
the lifting (∂Φ/∂X)λd, where λd=(f1d,f2d,λ1d,λ2d)T, and (3) the gradient ∂P/∂X of the
potential. In other words, (113) implies that if the artificial potential P attains its
minimum and, at the same time, makes the gradient ∂P/∂X vanish at some s=s*(s=(s1,s2)T), and, moreover, the artificial potential
is positive definite in X under the two
holonomic constraints, then the equilibrium position that minimizes P would be
asymptotically stable. Unfortunately, P is a quadratic
function with respect to only θ, p1, and p2, and, therefore, P is only
nonnegative definite in X. Nevertheless, it is possible to show that the
Hessian matrix of P with respect to θ, p1, and p2 becomes
positive definite in these three variables as shown in Table
1 that is
calculated by partially differentiating the gradients ∂P/∂θ, ∂P/∂p1, and ∂P/∂p2 of (111) to
(112) with respect to θ, p1, and p2 again.
Apparently, the Hessian becomes positive definite provided that γi is chosen as
being of similar order of (ri/fd), and θi(si) remains in a
region such that |θi(si)|<π/6 for i=1,2. Then, by using a similar argument used in proving
the stability on a submanifold for DOF redundant systems [9, 13], a solution trajectory to
the Lagrange equation converges exponentially to the constraint equilibrium
manifold. In this paper, we omit the details of the argument, but
show a physical meaning of the gradient ∂P/θ together with ∂P/∂pi(i=1,2).Table 1
Hessian matrix
of the potentialP.
∂2P∂θ2=∂SN∂θ=fdr1+r2{∂l∂θ(r1sinθ1+r2sinθ2)−∂d∂θ(r1cosθ1+r2cosθ2)}=fdr1+r2{(r1sinθ1+r2sinθ2)2+(r1cosθ1+r2cosθ2)2}=fdr1+r2{r12+r22+2r1r2cos(θ1−θ2)},∂2P∂pi2=ri2{fdr1+r2+1γi},i=1,2,∂2P∂pi∂θ=−fdr1+r2{ri2+r1r2cos(θ1−θ2)},i=1,2,∂2P∂p2∂p1=−r1r2fdr1+r2cos(θ1−θ2).As shown in Figure9, all rotational motions of the
object emerge around axes that are perpendicular to the xy-plane. Here,
we consider the rotational moment that may emerge at the contact point P2 exerted by the
pressing force f1 to the object
from the other contact point P1. Another force f2 at P2 that presses
the object generates the torque around the z-axis at P1. The sum of these torques around the z-axis can be
expressed as(116)r1×f1+r2×f2=P1P2→×r1fdr1+r2n1+P2P1→×r2fdr1+r2n2=(X(s1)−X(s2)Y(s1)−Y(s2)0)×r1fdr1+r2(cosθ1−sinθ10)+(X(s2)−X(s1)Y(s2)−Y(s1)0)×r2fdr1+r2(−cosθ2sinθ20)=fdr1+r2(00−(X1−X2)(r1sinθ1+r2sinθ2)−(Y1−Y2)(r1cosθ1+r2cosθ2)), 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(117)(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)).Applying this relation to (116),
we see that(118)r1×f1+r2×f2=fdr1+r2(0,0,l∑i=1,2risinθi−d∑i=1,2ricosθi)T=(0,0,SN)T.Figure 9
The pressing
force to the object atP1 in the
direction n1 induces a
rotational moment around P2 and vice versa.As a summary of the argument, it is possible to
conclude that a solution trajectory to (114) converges asymptotically to 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
for robots with redundant degrees of freedom is presented from the standpoint of a Riemannian geometric approach. It is shown
that any supply of the constant pressing force lying in the image space of the
constraint gradient to the environment through joint actuations is not relevant
to motions in the kernel space orthogonally compliment to the image space. An
extension of problems of grasping and manipulation of rigid objects to the case
of 2-dimensional objects with arbitrary shape is also treated from the
viewpoint of Dirichlet-Lagrange's stability by introducing a nonnegative
definite Morse-Lyapunov function on a Riemannian manifold together with damping
shaping.



---

*Source: 892801-2009-03-24.xml*