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.

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 an ๐‘›-dimensional configuration space. Planning of robot motions has been investigated traditionally on the basis of kinematics on a configuration space as an ๐‘›-dim numerical space ๐‘๐‘› [1].

This paper first emphasizes a mathematical observation that, given a robot as a multibody mechanism with n degrees of freedom whose endpoint is free, the set of all its postures can be regarded as a Riemannian manifold (๐‘€, ๐‘”) associated with the Riemannian metric ๐‘” 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 ๐‘› 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 ๐‘š(=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 Figure 1 is described by the formula๐ป(๐‘ž)ฬˆ๐‘ž+๎‚†12ฬ‡๐ป(๐‘ž)+๐‘†(๐‘ž,ฬ‡๐‘ž)๎‚‡ฬ‡๐‘ž+๐‘”(๐‘ž)=๐‘ข,(1)where ๐‘ž=(๐‘ž1,๐‘ž2)๐‘‡ denotes the vector of joint angles, ๐ป(๐‘ž) denotesthe inertia matrix, ๐‘†(๐‘ž,ฬ‡๐‘ž)ฬ‡๐‘ž the gyroscopic force term including centrifugal and Coriolis forces, ๐‘”(๐‘ž) the gradient vector of a potential function ๐‘ƒ(๐‘ž) due to the gravity with respect to ๐‘ž, and ๐‘ข the joint torque generated by joint actuators [4]. It is well known that the inertia matrix ๐ป(๐‘ž) is symmetric and positive definite, and there exist a positive constant โ„Ž๐‘š together with a positive definite constant diagonal matrix ๐ป0 such thatโ„Ž๐‘š๐ป0โ‰ค๐ป(๐‘ž)โ‰ค๐ป0(2)for any ๐‘ž. It should be also noted that ๐‘†(๐‘ž,ฬ‡๐‘ž) is skew symmetric and linear and homogeneous in ฬ‡๐‘ž. More in detail, the ๐‘–๐‘—th entry of ๐‘†(๐‘ž,ฬ‡๐‘ž) denoted by ๐‘ ๐‘–๐‘— can be described in the form [3]๐‘ ๐‘–๐‘—=12๎‚ป๐œ•๐œ•๐‘ž๐‘—๎‚ต๐‘›๎“๐‘˜=1ฬ‡๐‘ž๐‘˜โ„Ž๐‘–๐‘˜๎‚ถโˆ’๐œ•๐œ•๐‘ž๐‘–๎‚ต๐‘›๎“๐‘˜=1ฬ‡๐‘ž๐‘˜โ„Ž๐‘—๐‘˜๎‚ถ๎‚ผ,(3)where ๐ป(๐‘ž)=(โ„Ž๐‘–๐‘—(๐‘ž)), from which it follows apparently that ๐‘ ๐‘–๐‘—=โˆ’๐‘ ๐‘—๐‘–. 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 ๐ป(๐‘ž) is a constant or a sinusoidal function of components of joint angle vector ๐‘ž. That is, each element of ๐ป(๐‘ž) and ๐‘”(๐‘ž) is differentiable of class ๐ถโˆž (infinitely differentiable in ๐‘ž).

When two joint angles ๐œƒ1 and ๐œƒ2 are given in ๐œƒ๐‘–โˆˆ(โˆ’๐œ‹,๐œ‹], ๐‘–=1,2, for the 2 DOF robot arm shown in Figure 1, the posture ๐‘(๐œƒ1,๐œƒ2) is determined naturally. Denote the set of all such possible postures by ๐‘€ and introduce a family of subsets of ๐‘€ such that, for any ๐‘โˆˆ๐‘€ with joint angles ๐‘=(๐œƒ1,๐œƒ2) and any number ๐›ผ>0, a set of all ๐‘โ€ฒ=(๐œƒ๎…ž1,๐œƒ๎…ž2) is defined as๐‘ˆ๐‘,๐›ผ=๎€ฝ๐‘๎…žโˆถโ€–โ€–๐‘๎…žโˆ’๐‘โ€–โ€–๐ป<๐›ผ๎€พ,(4)whereโ€–โ€–๐‘๎…žโˆ’๐‘โ€–โ€–๐ป=๎ƒŽ๎“๐‘–,๐‘—โ„Ž๐‘–๐‘—(๐‘)๎€ท๐œƒ๎…ž๐‘–โˆ’๐œƒ๐‘–๎€ธ๎€ท๐œƒ๎…ž๐‘—โˆ’๐œƒ๐‘—๎€ธ(5)can be regarded as an open subset of ๐‘€. Then, the set ๐‘€ with such a family of open subsets can be regarded as a topological manifold. It is possible to show that the manifold ๐‘€ becomes Hausdorff and compact. Further, every point ๐‘ of ๐‘€ has a neighborhood ๐‘ˆ that is homeomorphic to an open subset ฮฉ of 2-dimensional numerical space ๐‘2. Such a homeomorphism ๐œ™โˆถ๐‘ˆโ†’ฮฉ is called a coordinate chart. In fact, a neighborhood ๐‘ˆ๐‘,๐›ผ of posture ๐‘ with joint angles (๐œƒ1,๐œƒ2) in Figure 1 can be mapped to an open set ฮฉ in ๐‘2 with 2 dimensional numerical coordinates (๐‘ž1,๐‘ž2) with the origin ๐‘‚ (Figure 2). In this case, it is possible to see that the original set ๐‘€ of robot postures can be visualized as a torus shown in ๐‘3 (see Figure 3) in which angles ๐‘ž1 and ๐‘ž2 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 (๐‘ž1,โ€ฆ,๐‘ž๐‘›) can be identically chosen as a set of ๐‘› independent joint angles (๐œƒ1,โ€ฆ,๐œƒ๐‘›) by setting ๐‘ž๐‘–=๐œƒ๐‘–(๐‘–=1,โ€ฆ,๐‘›). 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 ๐‘ƒ=(๐‘ฅ,๐‘ฆ,๐‘ง). As discussed in detail in mathematical text books [5, 6], the topological manifold (๐‘€,๐‘) of such a torus can be regarded as a differentiable manifold of class ๐ถโˆž.

Now, it is necessary to define a tangent vector to an abstract differentiable manifold ๐‘€ at ๐‘โˆˆ๐‘€. Let ๐ผ be an interval (โˆ’๐œ–,๐œ–) and define a curve ๐‘(๐‘ก) by a mapping ๐‘โˆถ๐ผโ†’๐‘€ such that ๐‘(0)=๐‘. A tangent vector to ๐‘€ at ๐‘ is an equivalence class of curves ๐‘โˆถ๐ผโ†’๐‘€ for the equivalence relation โˆผ defined by

๐‘โˆผ๐‘if and only if, in a coordinate chart (๐‘ˆ,๐œ™) around ๐‘, we have (๐œ™โˆ˜๐‘)๎…ž(0)=(๐œ™โˆ˜๐‘)๎…ž(0), where symbol ()โ€ฒ means differentiation of () with respect to ๐‘กโˆˆ๐ผ. This definition of tangent vectors to ๐‘€ at ๐‘ does not depend on choice of the coordinate chart at ๐‘, as discussed in text books [5, 6]. Let us denote the set of all tangent vectors to ๐‘€ at ๐‘ by ๐‘‡๐‘๐‘€ and call it the tangent space at ๐‘โˆˆ๐‘€. It has an ๐‘›-dimensional linear space structure like ๐‘๐‘›. We also denote the disjoint union of the tangent spaces to ๐‘€ at all the points of ๐‘€ by ๐‘‡๐‘€ and call it the tangent bundle of ๐‘€.

Now, we are in a position to define a Riemannian metric on a differentiable manifold (๐‘€,๐‘) as a mapping ๐‘”๐‘โˆถ๐‘‡๐‘๐‘€ร—๐‘‡๐‘๐‘€โ†’๐‘ such that ๐‘โ†’๐‘”๐‘ is of class ๐ถโˆž and ๐‘”๐‘(๐‘ข,๐‘ฃ) for ๐‘ขโˆˆ๐‘‡๐‘๐‘€ and ๐‘ฃโˆˆ๐‘‡๐‘๐‘€ is a symmetric positive definite quadratic form๐‘”๐‘(๐‘ข,๐‘ฃ)=๐‘›๎“๐‘–,๐‘—=1๐‘”๐‘–๐‘—(๐‘)๐‘ข๐‘–๐‘ฃ๐‘—.(6)

Suppose that ๐‘€ is a connected Riemannian manifold. If ๐‘โˆถ๐ผ[๐‘Ž,๐‘]โ†’๐‘€ is a curve segment of class ๐ถโˆž, we define the length of ๐‘ to be๎€œ๐ฟ(๐‘)=๐‘๐‘Žโ€–โ€–โ€–โ€–=๎€œฬ‡๐‘(๐‘ก)d๐‘ก๐‘๐‘Ž๎”๐‘”๐‘(๐‘ก)๎€ท๎€ธฬ‡๐‘(๐‘ก),ฬ‡๐‘(๐‘ก)d๐‘ก,(7)where we assume ฬ‡๐‘(๐‘ก)โ‰ 0 for any ๐‘กโˆˆ๐ผ and call such a curve segment to be regular. A mapping of class ๐ถโˆž๐‘โˆถ[๐‘Ž,๐‘]โ†’๐‘€ is called a piecewise regular curve segment (for brevity, we call it an admissible curve) if there exists a finite subdivision ๐‘Ž=๐‘Ž0<๐‘Ž1<โ‹ฏ<๐‘Ž๐‘˜=๐‘ such that ๐‘(๐‘ก) for ๐‘กโˆˆ[๐‘Ž๐‘–โˆ’1,๐‘Ž๐‘–] is a regular curve for ๐‘–=1,โ€ฆ,๐‘˜. Then, it is possible to define for any pair of points ๐‘,๐‘โ€ฒโˆˆ๐‘€ the Riemannian distance ๐‘‘(๐‘,๐‘๎…ž) to be the infimum of all admissible curves from ๐‘ to ๐‘๎…ž. It is well known [4, 5] that, with the distance function ๐‘‘ defined above, any connected Riemannian manifold becomes a metric space whose induced topology is coincident with the given manifold topology. An admissible curve ๐‘ in a Riemannian manifold is said to be minimizing if ๐ฟ(๐‘)โ‰ค๐ฟ(ฬƒ๐‘) for any other admissible curve ฬƒ๐‘ with the same endpoints. It follows immediately from the definition of distance that ๐‘ is minimizing if and only if ๐ฟ(๐‘) is equal to the distance between its endpoints. Further, it is known that if the Riemannian manifold {๐‘€,๐‘”} is complete, then for any pair of points ๐‘ and ๐‘๎…ž there exists at least a minimizing curve ๐‘(๐‘ก), ๐‘กโˆˆ[๐‘Ž,๐‘], with ๐‘(๐‘Ž)=๐‘ and ๐‘(๐‘)=๐‘๎…ž. If such a minimizing curve ๐‘(๐‘ก) is described with the aid of coordinate chart (๐‘ˆ,๐œ™) as ๐œ™(๐‘(๐‘ก))=(๐‘ž1(๐‘ก),โ€ฆ,๐‘ž๐‘›(๐‘ก)), then ๐‘ž(๐‘ก)=๐œ™(๐‘(๐‘ก)) satisfies the 2nd-order differential equationd2d๐‘ก2๐‘ž๐‘˜(๐‘ก)+๐‘›๎“๐‘–,๐‘—=1ฮ“๐‘˜๐‘–๐‘—(๐‘(๐‘ก))d๐‘ž๐‘–(๐‘ก)d๐‘กd๐‘ž๐‘—(๐‘ก)d๐‘ก=0,(๐‘˜=1,โ€ฆ,๐‘›),(8)where ฮ“๐‘˜๐‘–๐‘— denotes Christoffel's symbol defined byฮ“๐‘˜๐‘–๐‘—=12๐‘›๎“โ„Ž=1๐‘”๐‘˜โ„Ž๎‚ต๐œ•๐‘”๐‘–โ„Ž๐œ•๐‘ž๐‘—+๐œ•๐‘”๐‘—โ„Ž๐œ•๐‘ž๐‘–โˆ’๐œ•๐‘”๐‘–๐‘—๐œ•๐‘žโ„Ž๎‚ถ,(9)and (๐‘”๐‘˜โ„Ž) denotes the inverse of matrix (๐‘”๐‘˜โ„Ž). A curve ๐‘ž(๐‘ก)โˆถ๐ผโ†’๐‘ˆ satisfying (8) together with ๐œ™โˆ’1(๐‘ž(๐‘ก)) is called a geodesic, and (8) itself is called the Euler-Lagrange equation or the geodesic equation.

Given a ๐ถโˆž-class curve ๐‘(๐‘ก)=๐ผ[๐‘Ž,๐‘]โ†’๐‘€, the quantity1๐ธ(๐‘)=2๎€œ๐‘๐‘Žโ€–โ€–โ€–โ€–ฬ‡๐‘(๐‘ก)2=1d๐‘ก2๎€œ๐‘๐‘Ž๐‘”๐‘(๐‘ก)๎€ท๎€ธฬ‡๐‘(๐‘ก),ฬ‡๐‘(๐‘ก)d๐‘ก(10)is called the energy of the curve. Then, by applying the Cauchy-Schwartz inequality for (7), we have๐ฟ(๐‘)2โ‰ค2(๐‘โˆ’๐‘Ž)๐ธ(๐‘).(11)Further, the equality of (11) follows if and only if โ€–ฬ‡๐‘(๐‘ก)โ€– is constant. It is also possible to see that if ๐‘(๐‘ก) is a geodesic with ๐‘(๐‘Ž)=๐‘ and ๐‘(๐‘)=๐‘๎…ž, then for any other curve ๐‘(๐‘ก) with the same endpoints, it holds๐ธ(๐‘)=๐ฟ(๐‘)2โ‰ค๐ฟ๎€ท2(๐‘โˆ’๐‘Ž)๐‘๎€ธ2๎€ท2(๐‘โˆ’๐‘Ž)โ‰ค๐ธ๐‘๎€ธ.(12)The equalities hold if and only if ๐‘(๐‘ก) is also a geodesic. Conversely, if ๐‘(๐‘ก) with ๐‘(๐‘Ž)=๐‘ and ๐‘(๐‘)=๐‘๎…ž is a ๐ถโˆž curve that minimizes the energy and makes ๐‘”๐‘(๐‘ก)(ฬ‡๐‘(๐‘ก),ฬ‡๐‘(๐‘ก)) constant, then ๐‘(๐‘ก) becomes a geodesic connecting ๐‘(๐‘Ž)=๐‘ and ๐‘(๐‘)=๐‘๎…ž. In mechanics, ๐ธ(๐‘) is usually called โ€œaction of ๐‘,โ€ and ๐‘(๐‘ก) is considered as the orbit of motion of a multibody system.

3. Riemannian Geometry of Robot Dynamics

Dynamics of a robotic mechanism with ๐‘› 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 ๐‘ง-axis that is perpendicular to the ๐‘ฅ๐‘ฆ-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๎‚†1๐ป(๐‘ž)ฬˆ๐‘ž+2ฬ‡๎€ท๎€ธ๎‚‡๐ป(๐‘ž)+๐‘†๐‘ž,ฬ‡๐‘žฬ‡๐‘ž=๐‘ข,(13)where ๐‘ข 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 ๐‘”(๐‘ž) (included in (1)) can be compensated by joint actuators through control input ๐‘ข. In general, we can represent a posture ๐‘ of the robot as a physical entity by a family of joint angles ๐œƒ๐‘–(๐‘–=1,โ€ฆ,๐‘›), which can be expressed by a point ฮ˜=(๐œƒ1,โ€ฆ,๐œƒ๐‘›) in the ๐‘›-dimensional numerical space ๐‘๐‘›. In fact, we can naturally imagine an isomorphism ๐œ™โˆถ๐‘ˆโ†’ฮฉ, where ๐‘ˆโŠ‚๐‘€, and ฮฉ is an open subset of ๐‘๐‘›. In other words, a local coordinate chart ๐œ™(๐‘ˆ)(=ฮฉ) can be treated to be identical to ๐‘ˆ itself, an open subset of ๐‘€, by regarding ๐‘ž=(๐‘ž1,โ€ฆ,๐‘ž๐‘›)๐‘‡ (โ€œ๐‘‡โ€ denotes transpose and hence ๐‘ž a column vector) identical to ฮ˜ by setting ๐‘ž๐‘–=๐œƒ๐‘–(๐‘–=1,โ€ฆ,๐‘›). In this way, the abstract manifold ๐‘€ as the set of all robot postures can be regarded as an ๐‘›-dimensional torus ๐‘‡๐‘› as an ๐‘›-tuple direct product of ๐‘†1โˆถ๐‘‡๐‘›=๐‘†1ร—โ‹ฏร—๐‘†1. Hence, a robot posture ๐‘โˆˆ๐‘€ can be represented by a point ฮ˜ on ๐‘‡๐‘› and also expressed by a joint vector ๐‘ž in ๐‘๐‘›.

From the definition of inertia matrices, ๐ป(๐‘ž) is symmetric and positive definite, and the kinetic energy is expressed as a quadratic form๐พ๎€ท๎€ธ=1๐‘ž,ฬ‡๐‘ž2ฬ‡๐‘ž๐‘‡๐ป(๐‘ž)ฬ‡๐‘ž.(14)Hence, the equation of motion of the robot is expressed by Lagrange's equationd๎‚†๐œ•d๐‘ก๐ฟ๎€ท๎€ธ๎‚‡โˆ’๐œ•๐œ•ฬ‡๐‘ž๐‘ž,ฬ‡๐‘ž๐ฟ๎€ท๎€ธ๐œ•๐‘ž๐‘ž,ฬ‡๐‘ž=๐‘ข,(15)where ๐ฟ(๐‘ž,ฬ‡๐‘ž)=๐พ(๐‘ž,ฬ‡๐‘ž), and ๐‘ข stands for a generalized external force vector. It is interesting to note that in differential geometry, (15) can be described as๎“๐‘–โ„Ž๐‘˜๐‘–ฬˆ๐‘ž๐‘–+๎“๐‘–,๐‘—ฮ“๐‘–๐‘˜๐‘—(๐‘ž)ฬ‡๐‘ž๐‘–ฬ‡๐‘ž๐‘—=๐‘ข๐‘˜,(16)where ฮ“๐‘–๐‘˜๐‘— denotes Christoffel's symbol of the first kind defined byฮ“๐‘–๐‘˜๐‘—=12๎‚ต๐œ•โ„Ž๐‘—๐‘˜๐œ•๐‘ž๐‘–+๐œ•โ„Ž๐‘–๐‘˜๐œ•๐‘ž๐‘—โˆ’๐œ•โ„Ž๐‘–๐‘—๐œ•๐‘ž๐‘˜๎‚ถ.(17)For later use, we introduce another Christoffel's symbol called the second kind as shown in the formulaฮ“๐‘˜๐‘–๐‘—=12๐‘›๎“๐‘™=1โ„Ž๐‘™๐‘˜๎‚ต๐œ•โ„Ž๐‘—๐‘™๐œ•๐‘ž๐‘–+๐œ•โ„Ž๐‘–๐‘™๐œ•๐‘ž๐‘—โˆ’๐œ•โ„Ž๐‘–๐‘—๐œ•๐‘ž๐‘™๎‚ถ=12๐‘›๎“๐‘™=1โ„Ž๐‘™๐‘˜ฮ“๐‘–๐‘™๐‘—,(18)where (โ„Ž๐‘™๐‘˜) denotes the inverse of (โ„Ž๐‘™๐‘˜), the inertia matrix ๐ป(๐‘ž)=(โ„Ž๐‘™๐‘˜). Since (โ„Ž๐‘˜๐‘™) and (โ„Ž๐‘˜๐‘™) are symmetric, it follows that ฮ“๐‘–๐‘˜๐‘—=ฮ“๐‘—๐‘˜๐‘– and ฮ“๐‘˜๐‘–๐‘—=ฮ“๐‘˜๐‘—๐‘–. Now, we show that (13) is equivalent to (16) by bearing in mind that ฬ‡โˆ‘๐ป(๐‘ž)=๐‘–{๐œ•๐ป(๐‘ž)/๐œ•๐‘ž๐‘–}ฬ‡๐‘ž๐‘–, and the skew symmetric matrix ๐‘†(๐‘ž,ฬ‡๐‘ž) 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๐‘›๎“๐‘—=1๐‘ ๐‘˜๐‘—ฬ‡๐‘ž๐‘—=๐‘›๎“๐‘—=112๐œ•๎‚ธ๎‚ป๐œ•๐‘ž๐‘—๎‚ต๐‘›๎“๐‘–=1ฬ‡๐‘ž๐‘–โ„Ž๐‘˜๐‘–๎‚ถ๎‚ผฬ‡๐‘ž๐‘–โˆ’๎‚ป๐œ•๐œ•๐‘ž๐‘˜๎‚ต๐‘›๎“๐‘–=1ฬ‡๐‘ž๐‘–โ„Ž๐‘–๐‘—๎‚ถ๎‚ผฬ‡๐‘ž๐‘—๎‚น=๐‘›๎“๐‘›๐‘—=1๎“๐‘–=112๎‚ป๎‚ต๐œ•โ„Ž๐‘–๐‘˜๐œ•๐‘ž๐‘—โˆ’๐œ•โ„Ž๐‘–๐‘—๐œ•๐‘ž๐‘˜๎‚ถ๎‚ผฬ‡๐‘ž๐‘–ฬ‡๐‘ž๐‘—.(19)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 ๐ปโˆ’1(๐‘ž) yieldsฬˆ๐‘ž๐‘˜+๎“๐‘–,๐‘—ฮ“๐‘˜๐‘–๐‘—ฬ‡๐‘ž๐‘–ฬ‡๐‘ž๐‘—=๎“๐‘—โ„Ž๐‘˜๐‘—๐‘ข๐‘—,๐‘˜=1,โ€ฆ,๐‘›.(20)If ๐‘ข=0, this expression is nothing, but the Euler-Lagrange equation shown in (8). By this reason, from now on, we use symbol ๐‘”๐‘–๐‘—(๐‘ž) instead of โ„Ž๐‘–๐‘—(๐‘ž) for the inertia matrix ๐ป(๐‘ž) even when robot dynamics are treated.

Now, on the abstract topological manifold ๐‘€ 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 ๐‘‡๐‘๐‘€:โŸจ๐‘ฃ,๐‘คโŸฉ=๐‘”๐‘–๐‘—(๐‘)๐‘ฃ๐‘–๐‘ค๐‘—,(21)where ๐‘ฃ=๐‘ฃ๐‘–(๐œ•/๐œ•๐‘ž๐‘–)โˆˆ๐‘‡๐‘๐‘€ and ๐‘ค=๐‘ค๐‘—(๐œ•/๐œ•๐‘ž๐‘—)โˆˆ๐‘‡๐‘๐‘€, and the summation symbol โˆ‘ in ๐‘– and ๐‘— is omitted, and ๐‘ž=(๐‘ž1,โ€ฆ,๐‘ž๐‘›) represents local coordinates. Then, the manifold (๐‘€,๐‘) 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 ๐‘,๐‘žโˆˆ๐‘€ can be joined by a geodesic of length ๐‘‘(๐‘,๐‘ž), that is, a curve satisfying (8) with shortest length, where๎€œ๐‘‘(๐‘,๐‘ž)=inf๐‘๐‘Žโ€–โ€–โ€–โ€–๎€œฬ‡๐‘(๐‘ก)d๐‘ก=inf๐‘๐‘Ž๎”โŸจฬ‡๐‘(๐‘ก),ฬ‡๐‘(๐‘ก)โŸฉd๐‘ก(22)with ๐‘(๐‘Ž)=๐‘ and ๐‘(๐‘)=๐‘ž.

As discussed in the previous section, geodesics are the critical points of the energy functional ๐ธ(๐‘). Further, a geodesic curve ๐‘(๐‘ก) satisfies โ€–ฬ‡๐‘(๐‘ก)โ€–=const. In fact, by regarding ๐‘(๐‘ก)=๐‘ž(๐‘ก) that is an orbit on ฮฉ, we haveddd๐‘กโŸจฬ‡๐‘ž,ฬ‡๐‘žโŸฉ=๎€ท๐‘”d๐‘ก๐‘–๐‘—๎€ท๎€ธ๐‘ž(๐‘ก)ฬ‡๐‘ž๐‘–(๐‘ก)ฬ‡๐‘ž๐‘—๎€ธ=d(๐‘ก)๎€ฝd๐‘กฬ‡๐‘ž๐‘‡๎€พ(๐‘ก)๐บ(๐‘ž(๐‘ก))ฬ‡๐‘ž(๐‘ก)=ฬˆ๐‘ž๐‘‡(๐‘ก)๐บ(๐‘ž)ฬ‡๐‘ž(๐‘ก)+ฬ‡๐‘ž๐‘‡ฬ‡(๐‘ก)๐บ(๐‘ž)ฬ‡๐‘ž(๐‘ก)+ฬ‡๐‘ž๐‘‡(๐‘ก)๐บ(๐‘ž)ฬˆ๐‘ž(๐‘ก)=0,(23)where the equivalent expression๎‚†1๐บ(๐‘ž)ฬˆ๐‘ž+2ฬ‡๎€ท๎€ธ๎‚‡๐บ(๐‘ž)+๐‘†๐‘ž,ฬ‡๐‘žฬ‡๐‘ž=0(24)to (13) with ๐‘ข=0 is used, ๐บ(๐‘ž)=(๐‘”๐‘–๐‘—(๐‘ž)), and ๐‘ ๐‘–๐‘— of ๐‘† is given in the form of (3) (where โ„Ž๐‘–๐‘—=๐‘”๐‘–๐‘—). It is also important to note that, on a local coordinate chart ฮฉโŠ‚๐‘๐‘› corresponding to a neighborhood ๐‘ˆ of ๐‘โˆˆ๐‘€, an orbit ๐‘ž(๐‘ก) parameterized by time ๐‘กโˆˆ[๐‘Ž,๐‘] and expressed by a solution to (20) (where ๐‘ข๐‘— is of ๐ถโˆž in ๐‘ก) should satisfy๐‘›๎“๐‘—=1ฬ‡๐‘ž๐‘—(๐‘ก)๐‘ข๐‘—(๐‘ก)=dd๐‘ก๐ธ(๐‘ž(๐‘ก)),(25)or๎€œ๐‘๐‘Ž๐‘›๎“๐‘—=1ฬ‡๐‘ž๐‘—(๐‘ก)๐‘ข๐‘—(๐‘ก)d๐‘ก=๐ธ(๐‘ž(๐‘))โˆ’๐ธ(๐‘ž(๐‘Ž)),(26)as long as ๐‘ž(๐‘ก)โˆˆฮฉ, where ๐ธ(๐‘ž(๐‘ก))=(1/2)โŸจฬ‡๐‘ž(๐‘ก),ฬ‡๐‘ž(๐‘ก)โŸฉ. When ๐‘ข(๐‘ก)=0, ๐ธ(๐‘ž(๐‘ก))=const. and then the curve connecting ๐‘=๐œ™โˆ’1(๐‘ž(๐‘Ž)) and ๐‘๎…ž=๐œ™โˆ’1(๐‘ž(๐‘)) 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 an ๐‘›-dimensional torus ๐‘‡๐‘› is one of such choice of local coordinates corresponding to the choice of joint angles ๐‘ž=(๐‘ž1,โ€ฆ,๐‘ž๐‘›)๐‘‡. At the same time, it is important to note that, in differential geometry, choice of coordinates in the tangent space ๐‘‡๐‘๐‘€ is indeterminable or free to choose. However, once a local coordinates system for an ๐‘› degrees of freedom robot is chosen by joint angle vector ๐‘ž=(๐‘ž1,โ€ฆ,๐‘ž๐‘›)๐‘‡, then the coordinates in the tangent space ๐‘‡๐‘๐‘€ should be chosen as the vector of joint angular velocities ๐œ•/๐œ•๐‘ž=(๐œ•/๐œ•๐‘ž1,โ€ฆ,๐œ•/๐œ•๐‘ž๐‘›) correspondingly to ฬ‡๐‘ž=(ฬ‡๐‘ž1,โ€ฆ,ฬ‡๐‘ž๐‘›)๐‘‡, from which the Riemannian metric ๐‘”๐‘–๐‘—(๐‘ž) is defined through the inertia matrix.

4. Constraint Submanifold and Hybrid Position/Force Control

Consider an ๐‘›-DOF robotic arm whose last link is a pencil and suppose that the endpoint of the pencil is in contact with a flat surface ๐œ‘(๐ฑ)=๐œ‰, where ๐ฑ=(๐‘ฅ,๐‘ฆ,๐‘ง)๐‘‡. It is well known that the Lagrange equation of motion of the system is written as๐บ(๐‘ž)ฬˆ๐‘ž+๎‚†12ฬ‡๐บ+๐‘†๎‚‡ฬ‡๐‘ž+๐‘”(๐‘ž)=โˆ’๐œ†๐œ•๐œ‘(๐ฑ(๐‘ž))๐œ•๐‘ž+๐‘ข,(27)where ๐œ•๐œ‘/๐œ•๐‘ž can be decomposed into ๐œ•๐œ‘(๐‘ž)/๐œ•๐‘ž=๐ฝ๐‘‡(๐‘ž)๐œ•๐œ‘/๐œ•๐ฑ and ๐ฝ(๐‘ž)=๐œ•๐ฑ/๐œ•๐‘ž๐‘‡. On the constraint manifold ๐น๐œ‰={๐‘โˆถ๐‘โˆˆ๐‘€and๐œ‘(๐ฑ(๐‘))=๐œ‰}, let us consider a smooth curve ๐‘(๐‘ก)โˆถ๐ผ[๐‘Ž,๐‘]โ†’๐น๐œ‰ that connects the given two points ๐‘(๐‘Ž)=๐‘ and ๐‘(๐‘)=๐‘๎…ž, where ๐‘ and ๐‘๎…ž belong to ๐น๐œ‰. The length of such a curve constrained to ๐น๐œ‰ is defined as๐ฟ(๐‘)=๎€œ๐‘๐‘Ž๎”๐‘”๐‘–๐‘—(๐‘(๐‘ก))ฬ‡๐‘๐‘–(๐‘ก)ฬ‡๐‘๐‘—(๐‘ก)d๐‘ก,(28)and consider the minimization๐‘‘๎€ท๐‘,๐‘๎…ž๎€ธ=inf๐‘โˆˆ๐น๐œ‰๐ฟ(๐‘)(29)that should be called the distance between ๐‘ and ๐‘๎…ž on the constraint manifold. Then, the minimizing curve called the geodesic denoted identically by ๐‘ž(๐‘ก)(=๐‘(๐‘ก)) must satisfy the Euler equationฬˆ๐‘ž๐‘˜(๐‘ก)+ฮ“๐‘˜๐‘–๐‘—ฬ‡๐‘ž๐‘–(๐‘ก)ฬ‡๐‘ž๐‘—(๐‘ก)=โˆ’๐œ†(๐‘ก)โ‹…๎€ทgrad๐œ‘(๐ฑ(๐‘ก))๎€ธ๐‘˜,(30)together with the constraint condition ๐œ‘(๐ฑ(๐‘ก))=๐œ‰, wheregrad๐œ‘(๐ฑ(๐‘ก))=๐บโˆ’1(๐‘ž(๐‘ก))๐ฝ๐‘‡(๐‘ž)๐œ•๐œ‘๐œ•๐ฑ,(31)and ๐ฝ๐‘‡(๐‘ž)=๐œ•๐ฑ๐‘‡/๐œ•๐‘ž. It should be noted that, from the inner product of (30) and ๐‘ค=๐ฝ๐‘‡๐œ•๐œ‘/๐œ•๐ฑ, it follows that๎“๐‘˜๎€ท๐‘ค๐‘˜ฬˆ๐‘ž๐‘˜+๐‘ค๐‘˜ฮ“๐‘˜๐‘–๐‘—ฬ‡๐‘ž๐‘–ฬ‡๐‘ž๐‘—๎€ธ=โˆ’๐œ†๎“๐‘˜๐‘ค๐‘˜๐‘”๐‘˜๐‘—๐‘ค๐‘—.(32)Since the holonomic constraint ๐œ‘(๐ฑ(๐‘ž))=๐œ‰ implies โŸจ๐‘ค,ฬ‡๐‘žโŸฉ=0, it follows that4๎“๐‘˜=1๎‚€๐‘ค๐‘˜ฬˆ๐‘ž๐‘˜+๎‚€dd๐‘ก๐‘ค๐‘˜๎‚ฬ‡๐‘ž๐‘˜๎‚=0.(33)Substituting this into (32), we obtain๐œ†(๐‘ก)=1๐‘ค๐‘‡๐บโˆ’1๐‘ค๎‚ป๎“๐‘˜๎‚ตฬ‡๐‘ค๐‘˜ฬ‡๐‘ž๐‘˜โˆ’๎“๐‘–,๐‘—๐‘ค๐‘˜ฮ“๐‘˜๐‘–๐‘—ฬ‡๐‘ž๐‘–ฬ‡๐‘ž๐‘—๎‚ถ๎‚ผ.(34)From the Riemannian geometry, the constraint force ๐œ†(๐‘ก) with the grad{๐œ‘(๐ฑ(๐‘ž))} stands for a component of the image space of ๐‘ค(=๐ฝ๐‘‡(๐‘ž)๐œ•๐œ‘/๐œ•๐ฑ) that is orthogonal to the kernel ๐‘‡๐น๐œ‰ of ๐‘ค. 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, ๐œ†(๐‘ก) should be regarded as a magnitude of the constraint force that presses the surface ๐œ‘(๐ฑ(๐‘ž))=๐œ‰ 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)Then, by substituting this into (27), we obtain the Lagrange equation of motion under the constraint ๐œ‘(๐ฑ(๐‘ž))=๐œ‰:๐บ(๐‘ž)ฬˆ๐‘ž+๎‚†12ฬ‡๐บ+๐‘†๎‚‡ฬ‡๐‘ž=โˆ’๎€ท๐œ†โˆ’๐œ†๐‘‘๎€ธ๐ฝ๐‘‡(๐‘ž)๐œ•๐œ‘๐œ•๐ฑ=โˆ’ฮ”๐œ†๐ฝ๐‘‡(๐‘ž)๐œ•๐œ‘๐œ•๐ฑ,(36)where ฮ”๐œ†=๐œ†โˆ’๐œ†๐‘‘. 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 ๐œ†(๐‘ก)(=๐œ†๐‘‘+ฮ”๐œ†(๐‘ก)) positive. In a mathematical sense, exertion of the joint torque ๐œ†๐‘‘๐ฝ๐‘‡(๐œ•๐œ‘/๐ฑ) 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ฬ‡๐‘ž=๎€ท๐‘ƒ,๐‘คโ€–๐‘คโ€–โˆ’1๎€ธ๎‚ตฬ‡๐œผฬ‡๐‘ง๎‚ถ=๐‘„ฬ‡๐‘ž,(37)where ๐‘ƒ is a 4ร—3 matrix whose column vectors with the unit norm are orthogonal to ๐‘ค, and ฬ‡๐œผ denotes a 3ร—1 matrix (3-dim. vector) and ฬ‡๐‘ง a scalar. Since ๐‘„ is an orthogonal matrix, ๐‘„โˆ’1=๐‘„๐‘‡. Hence, if ฬ‡๐‘žโˆˆker(๐‘ค)(=๐‘‡๐น๐œ‰), then ฬ‡๐‘ง=0. Restriction of (36) to the kernel space of ๐‘ค can be attained by multiplying (36) by ๐‘ƒ๐‘‡ from the left such that๐‘ƒ๐‘‡๐บ(๐‘ž)dd๐‘ก(๐‘ƒฬ‡๐œผ)+๐‘ƒ๐‘‡๎‚†12ฬ‡๐บ+๐‘†๎‚‡๐‘ƒฬ‡๐œผ=0,(38)which is reduced to the Eular equation in ฬ‡๐œผ:๐บ(๐‘ž)ฬˆ๐œผ+๎‚†12ฬ‡๐บ+๐‘†๎‚‡ฬ‡๐œผ=0,(39)or equivalentlyฬˆ๐œ‚๐‘˜+ฮ“๐‘˜๐‘–๐‘—ฬ‡๐œ‚๐‘–ฬ‡๐œ‚๐‘—=0,๐‘˜=1,โ€ฆ,๐‘›โˆ’1(=3),(40)where ๐บ(๐‘ž)=๐‘ƒ๐‘‡๐บ(๐‘ž)๐‘ƒ, ฮ“๐‘˜๐‘–๐‘— the Christoffel symbol for (๐‘”๐‘–๐‘—)=๐บ, and๐‘†=๐‘ƒ๐‘‡๐‘†๐‘ƒโˆ’12ฬ‡๐‘ƒ๐‘‡๐บ๐‘ƒ+12๐‘ƒ๐‘‡๐บฬ‡๐‘ƒ,(41)which is skew symmetric, too. Note that the transformation ๐‘„ 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)where ๐œ‘(๐ฑ)=๐‘ง, ฬ‡๐ฑ=(ฬ‡๐‘ฅ,ฬ‡๐‘ฆ,0)๐‘‡, ฮ”๐ฑ=(๐‘ฅโˆ’๐‘ฅ๐‘‘,๐‘ฆโˆ’๐‘ฆ๐‘‘,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 ๐ฝ(๐‘ž) as 2ร—4 matrix ๐ฝ(๐‘ž)=(๐œ•๐‘ฅ/๐œ•๐‘ž๐‘‡,๐œ•๐‘ฆ/๐œ•๐‘ž๐‘‡) because we consider a special case of ๐œ‘(๐ฑ)=๐‘ง, that is, the constraint ๐‘งโˆ’๐œ‰=0, and solution trajectories ๐‘ž(๐‘ก) that satisfy ๐‘ง(๐‘ž(๐‘ก))โˆ’๐œ‰=0. In relation to this, denote also the two-dimensional vector (๐‘ฅ,๐‘ฆ) by ๐‘ฅ and (ฮ”๐‘ฅ,ฮ”๐‘ฆ) by ฮ”๐ฑ. It should be noted that the orthogonality relationship among (๐‘ฅ,๐‘ฆ,๐‘ง) coordinates in ๐„3 does not imply the orthogonality among ๐œ•๐‘ฅ/๐œ•๐‘ž, ๐œ•๐‘ฆ/๐œ•๐‘ž, and ๐œ•๐‘ง/๐œ•๐‘ž(=๐œ•๐œ‘/๐œ•๐‘ž). Therefore, ๐œ•๐‘ง/๐œ•๐‘ž is not orthogonal to ๐ฝ๐‘‡(๐‘ž)(=(๐œ•๐‘ฅ/๐œ•๐‘ž,๐œ•๐‘ฆ/๐œ•๐‘ž)), and hence the last term of the right hand side of (42) contains both components of image (๐‘ค) and ker(๐‘ค).

Now, substituting this into (27) yields๐บ(๐‘ž)ฬˆ๐‘ž+๎‚†12ฬ‡๐บ+๐‘†+๐ถ๎‚‡ฬ‡๐‘ž+๐ฝ๐‘‡(๐‘ž)๎€ฝ๐œโˆš๐‘˜ฬ‡๐ฑ+๐‘˜ฮ”๐ฑ๎€พ=โˆ’ฮ”๐œ†๐œ•๐‘ง(๐‘ž)๐œ•๐‘ž.(43)Evidently the inner product of (43) and ฬ‡๐‘ž under the constraint ๐‘ง(๐‘ž)=๐œ‰ leads todd๐‘ก๐ธ๎€ท๐‘ž,ฬ‡๐‘ž๎€ธ=โˆ’ฬ‡๐‘ž๐ถฬ‡๐‘žโˆ’๐œโˆš๐‘˜โ€–โ€–ฬ‡๐ฑโ€–โ€–2,(44)where๐ธ๎€ท๐‘ž,ฬ‡๐‘ž๎€ธ=12ฬ‡๐‘ž๐‘‡๐บ(๐‘ž)ฬ‡๐‘ž+๐‘˜2โ€–ฮ”๐ฑโ€–2.(45)Unfortunately, this quantity is not positive definite in the tangent bundle ๐‘‡๐น๐œ‰. Nevertheless, it is possible to see that magnitudes of ฬ‡๐‘ž(๐‘ก) and ฮ”๐ฑ(๐‘ก) remain small if at initial time ๐‘ก=0 both magnitudes โ€–ฬ‡๐‘ž(0)โ€– and โ€–ฮ”๐ฑ(0)โ€– are set small. Next, let us introduce the equilibrium manifold defined by the setEM1=๎€ฝ๐‘žโˆถ๐‘ง(๐‘ž)=๐œ‰,๐‘ฅ(๐‘ž)=๐‘ฅ๐‘‘,๐‘ฆ(๐‘ž)=๐‘ฆ๐‘‘๎€พ,(46)which is of one dimension. Fortunately as discussed in the paper [9], it is possible to confirm that a modified scalar function๐‘Š๐›ผ=๐ธ๎€ท๐‘ž,ฬ‡๐‘ž๎€ธ+๐›ผฬ‡๐‘ž๐‘‡๐บ(๐‘ž)๐‘ƒ๐œ‘๐ฝ๐‘‡(๐‘ž)ฮ”๐ฑ(47)becomes positive definite in ๐‘‡๐น๐œ‰ with an appropriate positive parameter ๐›ผ>0, provided that the physical scale of the robot is ordinary, and feedback gains ๐ถ and ๐‘˜ are chosen adequately. Here, in (47), ๐‘ƒ๐œ‘ is defined as ๐ผ4โˆ’๐‘ค๐‘ค๐‘‡/โ€–๐‘คโ€–2. Furthermore, if in a Riemannian ball in ๐น๐œ‰ defined as ๐ต(๐‘ž(0),๐‘Ÿ0)={๐‘žโˆถ๐‘‘(๐‘ž,๐‘ž(0))<๐‘Ÿ0}, the Jacobian matrix ๐ฝ(๐‘ž) is nondegenerate, then it can be expected that there exist positive numbers ๐œŽ๐‘š and ๐œŽ๐‘€ such that๐œŽ๐‘€๐ผ2โ‰ฅ๐ฝ๐ฝ๐‘‡โ‰ฅ๐ฝ๐‘ƒ๐œ‘๐ฝ๐‘‡โ‰ฅ๐œŽ๐‘š๐ผ2.(48)Further, to simplify the argument, we choose ๐ถ=๐‘๐ผ4 with a constant ๐‘>0. Then, it follows from differentiation of ๐‘‰(=ฬ‡๐‘ž๐‘‡๐บ(๐‘ž)๐‘ƒ๐œ‘๐ฝ๐‘‡(๐‘ž)ฮ”๐ฑ) thatฬ‡๐‘‰=โˆ’๎€ฝ๐œโˆš๐‘˜ฬ‡๐ฑ+๐‘˜ฮ”๐ฑ๎€พ๐‘‡๐ฝ๐‘ƒ๐œ‘๐ฝ๐‘‡ฮ”๐ฑโˆ’๐‘ฬ‡๐‘ž๐‘‡๐‘ƒ๐œ‘๐ฝ๐‘‡ฮ”๐ฑ+ฬ‡๐‘ž๐‘‡๐บ๐‘ƒ๐œ‘๐ฝ๐‘‡ฬ‡๐ฑโˆ’โ„Ž(ฬ‡๐‘ž,๐บ)ฮ”๐ฑ,(49)whereโ„Ž(ฬ‡๐‘ž,๐บ)=ฬ‡๐‘ž๐‘‡๎‚†๎‚€12ฬ‡๐บ๐‘ƒ๐œ‘โˆ’๐‘†๐‘ƒ๐œ‘โˆ’๐บฬ‡๐‘ƒ๐œ‘๎‚๐ฝ๐‘‡โˆ’๐บ๐‘ƒ๐œ‘ฬ‡๐ฝ๐‘‡๎‚‡.(50)This 1ร—2-vector โ„Ž is quadratic in components of ฬ‡๐‘ž, and each coefficient of ฬ‡๐‘ž๐‘–ฬ‡๐‘ž๐‘— (for ๐‘–,๐‘—=1,โ€ฆ,4) is at most of order of the maximum spectre radius of ๐บ(๐‘ž) denoted by ๐‘”๐‘€. Hence, it follows from (49) that||โ„Ž๎€ทฬ‡๐‘ž,๐บ๎€ธฮ”๐ฑ||โ‰ค๐‘”๐‘€๐‘™0โ€–โ€–ฬ‡๐‘žโ€–โ€–2โ€–ฮ”๐ฑโ€–,(51)where ๐‘™0 signifies a constant that is of order of the maximum link length (because the Jacobian matrix ๐œ•๐ฑ/๐œ•๐‘ž๐‘‡ is homogeneously related to the three link lengths of the robot shown in Figure 4). Further, note that ฬ‡๐‘ž๐‘‡๐‘ƒ๐œ‘=0 in this case and remark thatฬ‡๐‘ž๐‘‡๐บ๐‘ƒ๐œ‘๐ฝ๐‘‡ฬ‡๐ฑโ‰ค12ฬ‡๐‘ž๐‘‡๐บ๐บฬ‡๐‘ž+12ฬ‡๐ฑ๐‘‡๐ฝ๐‘ƒ๐œ‘๐ฝ๐‘‡ฬ‡๐ฑ,(52)โˆ’๐œโˆš๐‘˜ฬ‡๐ฑ๐‘‡๐ฝ๐‘ƒ๐œ‘๐ฝ๐‘‡ฮ”๐ฑโ‰ค๐œ๐‘˜2ฮ”๐ฑ๐‘‡๐ฝ๐‘ƒ๐œ‘๐ฝ๐‘‡ฮ”๐ฑ+๐œ2ฬ‡๐ฑ๐‘‡๎€ท๐ฝ๐ฝ๐‘‡๎€ธฬ‡๐ฑ.(53)Thus, substituting (51) to (52) into (49) yieldsฬ‡๐‘‰โ‰คโˆ’๎‚†๐‘˜โˆ’12โˆ’๐œ๐‘˜2๎‚‡ฮ”๐ฑ๐‘‡๐ฝ๐‘ƒ๐œ‘๐ฝ๐‘‡ฮ”๐ฑ+12ฬ‡๐‘ž๐‘‡๐บ๐บฬ‡๐‘ž+1+๐œ2ฬ‡๐ฑ๐‘‡๐ฝ๐ฝ๐‘‡ฬ‡๐ฑ+๐‘”๐‘€๐‘™0โ€–โ€–ฬ‡๐‘žโ€–โ€–2โ€–ฮ”๐ฑโ€–.(54)With the aid of a positive parameter ๐›ผ>0, it is easy to see that๐‘Š๐›ผโ‰ฅ๐ธโˆ’๐›ผ2ฬ‡๐‘ž๐‘‡๐บ(๐‘ž)๐บ(๐‘ž)ฬ‡๐‘žโˆ’๐›ผ2ฮ”๐ฑ๐‘‡๐ฝ๐‘ƒ๐œ‘๐ฝ๐‘‡ฮ”๐ฑ=12ฬ‡๐‘ž๐‘‡{๐บโˆ’๐›ผ๐บ๐บ}ฬ‡๐‘ž+12ฮ”๐ฑ๐‘‡๎€ท๐‘˜๐ผ2โˆ’๐›ผ๐ฝ๐‘ƒ๐œ‘๐ฝ๐‘‡๎€ธฮ”๐ฑ.(55)

Now, suppose that the robot has an ordinary physical scale with๐‘”๐‘€โ‰ค0.001[kgm2],๐‘™0โ‰ค0.2[m].(56)Then, it is possible to see thatฬ‡๐‘Š๐›ผ=ฬ‡๐ธ+๐›ผฬ‡๐‘‰โ‰คโˆ’๐›ผ๐‘˜๐œŽ๐‘š2โ€–ฮ”๐ฑโ€–2โˆ’๐‘3โ€–โ€–ฬ‡๐‘žโ€–โ€–2,(57)provided that โ€–ฮ”๐ฑ(๐‘ก)โ€–<0.3[m], (1+๐œ๐‘˜)โ‰ค๐‘˜, and ๐œโˆš๐‘˜>๐›ผ(1+๐œ)/2. From the practical point of view of control design for the handwriting robot, ๐œ is set around ๐œ=โˆš2/2, and ๐‘˜ is chosen between 5.0โˆผ20.0[kg/s2] (see [9]). On account of (48) and (56),๐‘Š๐›ผโ‰ฅ๎€ท1โˆ’๐›ผ๐›พ0๎€ธ๐ธ,(58)and similarly,๐‘Š๐›ผโ‰ค๎€ท1+๐›ผ๐›พ0๎€ธ๐ธ,(59)where ๐›พ0=max(๐‘”๐‘€,๐œŽ๐‘€/๐‘˜). Then, if the damping factor can be selected to satisfy both inequalities ๐‘โ‰ฅ(3/2)(๐›ผ๐œŽ๐‘š๐‘”๐‘€) and ๐‘>๐›ผ๐‘”๐‘€๐‘™0, then (57) is reduced toฬ‡๐‘Š๐›ผโ‰คโˆ’๐›ผ๐œŽ๐‘š1+๐›ผ๐›พ0๐‘Š๐›ผ,(60)where ๐›พ0 can be set as ๐›พ0=๐œŽ๐‘€/๐‘˜ which is larger than ๐‘”๐‘€. Hence, by choosing ๐›ผ=1/2๐›พ0=๐‘˜/2๐œŽ๐‘€, it follows from (60) and (59) that๐‘Š๐›ผ(๐‘ก)=๐‘Š๐›ผ(0)๐‘’โˆ’๐›พ๐‘ก,(61)where ๐›พ=๐‘˜๐œŽ๐‘š/3๐œŽ๐‘€.

Now, suppose that ๐‘žโˆ— in EM1โˆฉ๐ต(๐‘ž(0),๐‘Ÿ0) is the minimizing point that connects with ๐‘ž(0) among all geodesics from ๐‘ž(0) to any point of EM1โˆฉ๐ต(๐‘ž(0),๐‘Ÿ0). We call ๐‘žโˆ— a reference point corresponding to ๐‘ž(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 ๐ต(๐‘žโˆ—,๐›ฟ(๐œ€)) with ฬ‡๐‘ž(0)=0 remains inside ๐ต(๐‘žโˆ—,๐œ€) for any ๐‘ก>0, then the reference point ๐‘žโˆ— on EM1 is said to be stable on a submanifold (see Figure 5).
It can be concluded from the exponential convergence of ๐‘Š๐›ผ to zero and the inequality ๐ธโ‰ค2๐‘Š๐›ผ when ๐›ผ=1/2๐›พ0 that any point inside ๐ต(๐‘žโˆ—,๐›ฟ(๐œ€)) included in ๐ต(๐‘ž(0),๐‘Ÿ1) for some ๐‘Ÿ1(<๐‘Ÿ0) is stable on a submanifold and further such a solution trajectory converges asymptotically to some ๐‘žโˆž 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 .

It should be noticed from the proof in Appendix that asymptotic convergence of the solution trajectory to some ๐‘žโˆž on the equilibrium manifold implies also the asymptotic convergence of constraint force ๐œ†(๐‘ก) to ๐œ†๐‘‘ as ๐‘กโ†’โˆž because ฮ”๐œ†=๐œ†โˆ’๐œ†๐‘‘ is expressed asฮ”๐œ†(๐‘ก)=1๐‘ค๐‘‡๐บโˆ’1๐‘ค๎‚ป๎“๐‘˜๎‚ตฬ‡๐‘คฬ‡๐‘ž๐‘˜โˆ’๎“๐‘–,๐‘—๐‘ค๐‘˜ฮ“๐‘˜๐‘–๐‘—ฬ‡๐‘ž๐‘–ฬ‡๐‘ž๐‘—๎‚ถโˆ’๐‘ค๐‘‡๐ฝ๐‘‡๎€ท๐œโˆš๐‘˜ฬ‡๐ฑ+๐‘˜ฮ”๐ฑ๎€ธ๎‚ผ,(62)and this right hand side converges to zero as ๐‘กโ†’โˆž.

The stability notion of a Riemannian ball in a neighborhood of a reference equilibrium state ๐‘žโˆ— on EM1 is extended to cope with the case that the initial velocity vector ฬ‡๐‘ž(0) is not zero. To do this, we define an extended Riemannian ball in the tangent bundle ๐‘€ร—๐‘‡๐น๐œ‰ around (๐‘žโˆ—,ฬ‡๐‘ž=0) in such a way that๐ต๎€ฝ๎€ท๐‘žโˆ—,0๎€ธ;๎€ท๐‘Ÿ0,๐‘Ÿ๐พ๎€ธ๎€พ=๎‚†๎€ท๐‘ž,ฬ‡๐‘ž๎€ธโˆถ๐‘‘๎€ท๐‘ž,๐‘žโˆ—๎€ธ<๐‘Ÿ0,๎”(1/2)ฬ‡๐‘ž๐‘‡๐บ(๐‘ž)ฬ‡๐‘ž<๐‘Ÿ๐พ๎‚‡,(63)where ๐‘‘(๐‘ž,๐‘žโˆ—) denotes the distance between ๐‘ž and ๐‘žโˆ— restricted to the submanifold ๐น๐œ‰, and ๐บ is defined below (40).

Definition 2 (asymptotic stability on a submanifold). If for any ๐œ€>0, there exist numbers ๐›ฟ(๐œ€) and ๐›ฟ๐พ(๐œ€) such that any solution trajectory of (43) starting from an arbitrary initial position and velocity inside ๐ต{(๐‘žโˆ—,0);(๐›ฟ(๐œ€),๐›ฟ๐พ(๐œ€))} remains in ๐ต{(๐‘žโˆ—,0);(๐œ€,๐‘Ÿ๐พ)} and further converges asymptotically to some equilibrium point ๐‘žโˆžโˆˆ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 Figure 6. In this figure, the two robots are installed on the horizontal ๐‘ฅ๐‘ฆ-plane ๐„2. We denote the object mass center by ๐‘‚๐‘š with the coordinates (๐‘ฅ๐‘š,๐‘ฆ๐‘š) expressed in the inertial frame. On the other hand, we express a local coordinate system fixed at the object by ๐‘‚๐‘š-๐‘‹๐‘Œ together with unit vectors ๐ซ๐‘‹ and ๐ซ๐‘Œ along the ๐‘‹-axis and ๐‘Œ-axis, respectively (see Figure 7). The left-hand side surface of the object is expressed by a curve ๐‘(๐‘ ) with local coordinates (๐‘‹(๐‘ ),๐‘Œ(๐‘ )) in terms of arc length parameter ๐‘  as shown in Figure 7.

First, suppose that at the left-hand contact point ๐‘ƒ1 the fingertip of the left finger is contacting with the object. Denote the unit normal at ๐‘ƒ1 by ๐ง1 and the unit tangent vector by ๐ž1. Note that ๐ง1 is normal to both the object surface and finger-end sphere at ๐‘ƒ1, and ๐ž1 is tangent to them at ๐‘ƒ1, too. If we denote position ๐‘ƒ1 by local coordinates (๐‘‹(๐‘ ),๐‘Œ(๐‘ )) fixed at the object (see Figure 7), then the angle from the ๐‘‹-axis to the unit normal ๐ง1 is assumed to be determined by a function on the curve:๐œƒ1(๐‘ )=arct๎‚ต๐‘‹๎…ž(๐‘ )๐‘Œ๎…ž(๐‘ )๎‚ถ,(64)where ๐‘‹๎…ž(๐‘ )=d๐‘‹(๐‘ )/d๐‘  and ๐‘Œโ€ฒ=d๐‘Œ(๐‘ )/d๐‘ . In this paper, all angles are set positive in counterclockwise direction. Then,๐‘ƒ1๐‘ƒ๎…ž1=๐‘™๐‘›1(๐‘ )=โˆ’๐‘‹(๐‘ )cos๐œƒ1(๐‘ )+๐‘Œ(๐‘ )sin๐œƒ1(๐‘ ),(65)which is dependent only on ๐‘  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):๐‘ƒ1๐‘ƒ๎…ž1=๎€ท๐‘ฅ๐‘šโˆ’๐‘ฅ01๎€ธcos๎€ท๐œƒ+๐œƒ1(๐‘ )๎€ธโˆ’๎€ท๐‘ฆ๐‘šโˆ’๐‘ฆ01๎€ธsin๎€ท๐œƒ+๐œƒ1(๐‘ )๎€ธโˆ’๐‘Ÿ1.(66)Hence, the left-hand contact constraint can be expressed by the holonomic constraint๐‘„1=โˆ’๎€ท๐‘ฅ๐‘šโˆ’๐‘ฅ01๎€ธcos๎€ท๐œƒ+๐œƒ1๎€ธ+๎€ท๐‘ฆ๐‘šโˆ’๐‘ฆ01๎€ธsin๎€ท๐œƒ+๐œƒ1๎€ธ+๎€ท๐‘Ÿ1+๐‘™๐‘›1(๐‘ )๎€ธ=0,(67)where ๐‘™๐‘›1(๐‘ ) denotes the right-hand side of (65) and ๐œƒ1=๐œƒ1(๐‘ ) for abbreviation.

Next, we note that the length ๐‘‚๐‘š๐‘ƒ๎…ž1 can be regarded also as a shape function of the object given by๐‘‚๐‘š๐‘ƒ๎…ž1=๐‘™๐‘’1(๐‘ )=๐‘‹(๐‘ )sin๐œƒ1+๐‘Œ(๐‘ )cos๐œƒ1.(68)On the other hand, this quantity can be also expressed as๐‘‚๐‘š๐‘ƒ๎…ž1=๐‘Œ1(๐‘ก)=โˆ’๎€ท๐‘ฅ๐‘šโˆ’๐‘ฅ01๎€ธsin๎€ท๐œƒ+๐œƒ1๎€ธโˆ’๎€ท๐‘ฆ๐‘šโˆ’๐‘ฆ01๎€ธcos๎€ท๐œƒ+๐œƒ1๎€ธ.(69)As discussed in [13], in the light of the book [14], pure rolling at ๐‘ƒ1 is defined by the condition that the translational velocity of ๐‘ƒ1 on the finger sphere is equal to that of ๐‘ƒ1 on the object contour along the ๐ž1-axis. Hence, by denoting ๐‘1=๐‘ž11+๐‘ž12+๐‘ž13, we haveโˆ’๐‘Ÿ1ฬ‡๐‘1+๐‘Ÿ1ฬ‡๐œƒ=โˆ’ฬ‡๐‘Œ1=๎€ทฬ‡๐‘ฅ๐‘šโˆ’ฬ‡๐‘ฅ01๎€ธsin๎€ท๐œƒ+๐œƒ1๎€ธ+๎€ทฬ‡๐‘ฆ๐‘šโˆ’ฬ‡๐‘ฆ01๎€ธcos๎€ท๐œƒ+๐œƒ1๎€ธ+๎€ฝ๎€ท๐‘ฅ๐‘šโˆ’๐‘ฅ01๎€ธcos๎€ท๐œƒ+๐œƒ1๎€ธโˆ’๎€ท๐‘ฆ๐‘šโˆ’๐‘ฆ01๎€ธsin๎€ท๐œƒ+๐œƒ1)๎€พฬ‡๐œƒ,(70)which from (67) can be reduced toโˆ’๐‘Ÿ1ฬ‡๐‘1โˆ’๐‘™๐‘›1ฬ‡๐œƒ+๎€ทฬ‡๐‘ฅ01โˆ’ฬ‡๐‘ฅ๎€ธsin๎€ท๐œƒ+๐œƒ1๎€ธ+๎€ทฬ‡๐‘ฆ01โˆ’ฬ‡๐‘ฆ๎€ธcos๎€ท๐œƒ+๐œƒ1๎€ธ=0.(71)This constraint form is Pfaffian. As to the contact point ๐‘ƒ2 at the right-hand finger end, a similar nonholonomic constraint can be obtained. Thus, by introducing Lagrange's multipliers ๐‘“1 and ๐‘“2 associated with holonomic constraints ๐‘„๐‘–=0(๐‘–=1,2), it is possible to construct a Lagrangian:๐ฟ=12๎“ฬ‡๐‘ž๐‘‡๐‘–๐บ๐‘–๎€ท๐‘ž๐‘–๎€ธฬ‡๐‘ž๐‘–+12๎€ฝ๐‘€ฬ‡๐‘ฅ2+๐‘€ฬ‡๐‘ฆ2+๐ผฬ‡๐œƒ2๎€พโˆ’๐‘“1๐‘„1โˆ’๐‘“2๐‘„2,(72)where ๐‘ž๐‘– denote the joint vector for finger ๐‘–, ๐บ๐‘–(๐‘ž๐‘–) the inertia matrix for finger ๐‘–, ๐‘€ and ๐ผ 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 ๐‘ƒ2 with another multipliers ๐œ†๐‘–(๐‘–=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:๐ผฬˆ๐œƒโˆ’๐‘“1๐‘Œ1+๐‘“2๐‘Œ2โˆ’๐œ†1๐‘™๐‘›1+๐œ†2๐‘™๐‘›2=0,(73)๐‘€๎‚ตฬˆ๐‘ฅฬˆ๐‘ฆ๎‚ถโˆ’๐‘“1๐ง1โˆ’๐‘“2๐ง2โˆ’๐œ†1๐ž1โˆ’๐œ†2๐ž2=0,(74)๐บ๐‘–๎€ท๐‘ž๐‘–๎€ธฬˆ๐‘ž๐‘–+๎‚†12ฬ‡๐บ๐‘–+๐‘†๐‘–๎‚‡ฬ‡๐‘ž๐‘–+๐‘“๐‘–๐ฝ๐‘‡๐‘–๎€ท๐‘ž๐‘–๎€ธ๐ง๐‘–+๐œ†๐‘–๎€ฝ๐ฝ๐‘‡๐‘–๎€ท๐‘ž๐‘–๎€ธ๐ž๐‘–+(โˆ’1)๐‘–๐‘Ÿ๐‘–๐ฉ๐‘–๎€พ=๐‘ข๐‘–,๐‘–=1,2,(75) where ๐ฉ1=(1,1)๐‘‡, ๐ฉ2=(1,1,1)๐‘‡, and ๐ฝ๐‘‡๐‘–(๐‘ž๐‘–)=๐œ•(๐‘ฅ0๐‘–/๐‘ฆ0๐‘–)/๐œ•๐‘ž๐‘–, and๐ง๐‘–=(โˆ’1)๐‘–๎ƒฉโˆ’cos๎€ท๐œƒ+๐œƒ๐‘–๎€ธsin๎€ท๐œƒ+๐œƒ๐‘–๎€ธ๎ƒช,๐ž๐‘–=๎ƒฉsin๎€ท๐œƒ+๐œƒ๐‘–๎€ธcos๎€ท๐œƒ+๐œƒ๐‘–๎€ธ๎ƒช(76)for ๐‘–=1,2. It is important to note that the object dynamics of (73) and (74) can be recast in the form๐ป0ฬˆ๐ณ+๐‘“1๐ฐ1+๐‘“2๐ฐ2+๐œ†1๐ฐ3+๐œ†2๐ฐ4=0,(77)where ๐ณ=(๐‘ฅ,๐‘ฆ,๐œƒ), and๐ฐ๐‘–=๎‚ตโˆ’๐ง๐‘–(โˆ’1)๐‘–๐‘Œ๐‘–๎‚ถ,๐ฐ๐‘–+2=๎‚ตโˆ’๐ž๐‘–(โˆ’1)๐‘–๐‘™๐‘›๐‘–๎‚ถ,๐‘–=1,2,(78)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๐—=๎€ท๐‘ฅ,๐‘ฆ,๐œƒ,๐‘ž๐‘‡1,๐‘ž๐‘‡2๎€ธ๐‘‡,๐ฎ=๎€ท0,0,0,๐‘ข๐‘‡1,๐‘ข๐‘‡2๎€ธ๐‘‡,๐บ(๐—)=diag๎€ท๐‘€,๐‘€,๐ผ,๐บ1๎€ท๐‘ž1๎€ธ,๐บ2๎€ท๐‘ž2๎€ธ๎€ธ,๐‘†(๐—,ฬ‡๐—)=diag๎€ท0,0,0,๐‘†1,๐‘†2๎€ธ,(79)then (73) โˆผ (75) can be written in the form๐บ(๐—)ฬˆ๐—+๎‚†12ฬ‡๐บ+๐‘†๎‚‡ฬ‡๐—+๎“๐‘–=1,2๎‚ต๐‘“๐‘–๐œ•๐‘„๐‘–๐œ•๐—+๐œ†๐‘–๐œ•๐‘…๐‘–๐œ•๐—๎‚ถ=๐ฎ,(80)where๐‘…๐‘–=โˆ’(โˆ’1)๐‘–๐‘Ÿ๐‘–๎€ฝ๎€ท๐œƒ+๐œƒ๐‘–๎€ท๐‘ ๐‘–๎€ธ๎€ธโˆ’๐ฉ๐‘‡๐‘–๐‘ž๐‘–๎€พ+๐‘Œ๐‘–+๐œ“๐‘–๎€ท๐‘ ๐‘–๎€ธ,(81)and ๐œ“๐‘– signifies a function depending only on the shape parameter ๐‘ ๐‘–. 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 ๐ฎ=0 in (80), the image space of the Riemannian manifold (๐—,๐บ(๐—)) with constraints ๐‘„1=0 and ๐‘„2=0 is spanned from the gradients๐บโˆ’1(๐—)๎‚ต๐œ•๐‘„1๐œ•๐—,๐œ•๐‘„2๐œ•๐—,๐œ•๐‘…1๐œ•๐—,๐œ•๐‘…2๐œ•๐—๎‚ถ,(82)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 ๐‘–=2 induce further restriction of the tangent space to a more subdimensional linear space. Here, ๐œ•๐‘…๐‘–/๐œ•๐— should be taken at the time instant