Dynamically consistent Jacobian inverse for non-holonomic robotic systems

This paper presents an extension of the concept of dynamically consistent Jacobian inverse from robotic manipulators (holonomic systems) to non-holonomic robotic systems, like mobile robots. This new inverse is derived within the framework of the endogenous configuration space approach, following a strict analogy with the original derivation of the dynamically consistent Jacobian inverse for holonomic systems. The analogy is founded on replacing a finite-dimensional configuration space of the manipulation robot by the space of control functions steering the non-holonomic system. Consequently, a curve in the space of control functions corresponds to the manipulator’s trajectory in the configuration space, whereas endogenous velocities and forces are defined as elements of the tangent and cotangent spaces to the control space. Three ways of introducing the dynamically consistent Jacobian inverse are proposed, referred to as the geometric method, the force method, and the optimization method. A crucial concept underlying all these methods is a Riemannian metric in the space of control functions of the non-holonomic system as well as in its operational space. It has been shown that, similarly as for holonomic systems, the dynamically consistent Jacobian inverse obtained prevents the transmission of certain internal forces acting in the system from the endogenous configuration space to the operational space. This property is illustrated with the example of the Pioneer 2DX mobile platform. Performance of the new Jacobian inverse is demonstrated in the context of motion planning of the rolling ball.


Introduction
The concept of a dynamically consistent Jacobian inverse for manipulation robots was founded by Khatib, see [11,12]; it is also known as the inertia weighted Jacobian pseudoinverse [10,15]. The dynamic consistency of this inverse consists in decoupling a force that acts in the configuration space into a force coming from the operational space, and an internal force that affects solely the motion in the configuration space. The concept of this inverse was supported by further theoretical arguments [5,8] and applied to the operational control of holonomic systems, including redundant manipulators [15], mobile manipulators with holonomic base [4], humanoid robots [13,19], and biomechanical systems [6,7]. An incorporation of the constraint consistent inertia matrix into a weighted Jacobian inverse for non-holonomic wheeled-based mobile manipulators has been proposed in [16,25]; an inherent flaw of such an approach in the context of general nonholonomic systems was revealed in [26]. In this last reference, the endogenous configuration space approach was suggested as an alternative. The dynamically consistent Jacobian inverse serves as a tool for solving the inverse kinematics or the motion planning problem in the way that respects some basic requirements of the force transmission from the configuration to the operational space of the robot. More specifically, since the robot's Jacobian can be regarded as a transformation of velocities from the configuration (joint) to the operational (task) space, therefore a right Jacobian inverse transforms velocities from the operational to the configuration space. Consequently, the dual Jacobian inverse transmits forces acting in the configuration space to the operational space, so in general, the forces exerted in the configuration space will produce a motion of the end effector in the operational space. A Jacobian inverse is called dynamically consistent, if forces belonging to the null space of the dual Jacobian inverse are not transmitted to the operational space, so they do not affect the motion of the end effector. This feature distinguishes the dynamic consistency.
For completeness and clarity of presentation, this paper begins with three methods of derivation of the dynamically consistent Jacobian inverse for holonomic robots. All these methods rely on Riemannian metrics in the configuration space and in the operational space, defined by the inertia matrix of the robot dynamics. In doing so, we approach the subject of Lagrangian mechanics on infinite-dimensional configuration spaces [9,14]. The geometric method assumes commutativity of a diagram of maps between tangent and cotangent spaces to the configuration and the operational spaces. The force method reconstructs original Khatib's construction. The optimization method designs the dynamically consistent Jacobian inverse as a solution of a constrained minimization problem of the robot's kinetic energy. Next, the same three methods are applied to non-holonomic systems. It is assumed that the kinematics of a non-holonomic robotic system are represented by a driftless control system with output, and the endogenous configuration space approach developed in [22,23,26] is adopted. Conceptually, this approach is rooted in the continuation method traced back to Ważewski [24], and introduced to robotics by Sussmann [21]. A fundamental premise of the endogenous configuration space approach is an analogy between the configuration of a manipulator and the control function of the non-holonomic system; for this reason, the latter is called the endogenous configuration. This analogy encompasses the manipulator's kinematics and the endpoint map of the control system representation of kinematics of the non-holonomic system. As a consequence, the non-holonomic system Jacobian is computed by differentiating the kinematics, and a right Jacobian inverse can be introduced as an inverse of a map between Hilbert spaces.
In the presence of the non-holonomic constraints, the dynamics equations of the system are obtained on the basis of d'Alembert's Principle. The inertia matrix computed along the system's trajectory defines a Riemannian metric in the endogenous configuration space. In this setting, the derivation of the dynamically consistent Jacobian inverse for a non-holonomic system is patterned on the derivation performed for the holonomic systems. Specifically, a curve in the endogenous configuration space corresponds to the manipulator's trajectory in the configuration space, whereas endogenous velocities and forces can be defined as elements of the tangent and cotangent spaces to the endogenous configuration space. The geometric, force, and optimization methods of defining the dynamically consistent Jacobian inverse have been developed. In order to demonstrate and compare the property of dynamic consistency, an immobilization problem has been solved, for example, holonomic and non-holonomic systems, acted on by some internal force, by means of first the dynamically consistent Jacobian inverse and then the usual Jacobian pseudoinverse. Transmission of this force to the operational space has not taken place only in case of the dynamically consistent inverse. Finally, in order to illustrate performance of the dynamically consistent Jacobian inverse, a motion planning problem for the rolling ball has been solved using these two inverses. An advantages of the dynamically consistent Jacobian inverse consist in avoiding representation singularities and smoothing the motion paths.
The presentation of this paper uses some very basic concepts of analytic mechanics and differential geometry, abundantly provided in [1]. The material concerned with control theory and non-holonomic systems is covered by [3,20]. The reference [22] may serve as an introduction to the endogenous configuration space approach. The concept of dynamically consistent Jacobian inverse was first announced by these authors in a conference paper [18]. Then, in [17], an application of the force method to the design of the dynamically consistent Jacobian inverse for mobile manipulators was proposed.
This paper is intended as a comprehensive introduction to dynamically consistent Jacobian inverses for holonomic and non-holonomic systems. Its main contribution is the following: 1. Introduction of three methods of deriving the dynamically consistent Jacobian inverse for holonomic systems. 2. Extension of these methods to non-holonomic systems. 3. Definition of the Riemannian metric and related concepts for non-holonomic systems. 4. Derivation of the dynamically consistent Jacobian inverse for non-holonomic systems. 5. Demonstration of dynamical consistency for holonomic and non-holonomic systems. 6. Application of the dynamically consistent Jacobian inverse to motion planning.
The composition of this paper is the following. In Sect. 2, we restate a standard derivation of the equations of motion for a non-holonomic system. Section 3 restitutes Khatib's dynamically consistent Jacobian inverse for holonomic systems. Section 4 proposes a derivation of the dynamically consistent Jacobian inverse for nonholonomic systems. Section 5 demonstrates computational examples. The paper is concluded with Sect. 6.

Basic concepts
We shall study a robotic system described by generalized coordinates q ∈ Q = R n , and velocitiesq ∈ T q Q, subject to l < n independent phase constraints in the Pfaffian form where T q Q ∼ = R n denotes the tangent space to Q at q and A(q) is a constraints matrix of dimension l × n and rank l.
Our main focus will be the kinematics of the system that can be represented as a driftless control system with output function where vector fields g i (q) span the null space of A(q), u ∈ R m , m = n−l, is a control vector, and y ∈ Y = R r denotes operational coordinates. It will be assumed that admissible control functions of (2) belong to a linear subspace X ⊂ L 2 m [0, T ] of the Lebesgue square integrable functions defined on [0, T ] equipped with the inner product < u 1 (·), u 2 (·) >= T 0 u T 1 (t)u 2 (t)dt, and are such that for every u(·) ∈ X the trajectory q(t) = ϕ q 0 ,t (u(·)) of (2) exists over the interval [0, T ]. The initial state q 0 of the system (2) and the control time horizon T > 0 are regarded as fixed. Following [22], the space X will be called the endogenous configuration space of the robotic system.
Let g X (u(·)) : T u(·) X × T u(·) X −→ R, where T u(·) X ∼ = X , be a Riemannian metric on the endogenous configuration space X , and g Q (q) : Riemannian metrics, respectively, on the configuration space and on the operational space. These metrics will be specified and exploited later on. The endpoint map of the system (2) determines the operational space point reached from q 0 at T under the action of the control function u(·), K q 0 ,T (u(·)) = k(q(T )) = k(ϕ q 0 ,T (u(·))). ( The map (3) will be identified with the kinematics of (2). The derivative of the kinematics, T K q 0 ,T (u(·)) Y ∼ = R r being the tangent space to Y at the point K q 0 ,T (u(·)), will be referred to as the Jacobian of the robotic system. The Jacobian is determined by the linear approximation to the system (2) along the control-trajectory pair (u(t), q(t)), see [20], initialized at ξ 0 = 0, where ξ ∈ R n , v ∈ R m , w ∈ R r , and Given a control v(·) ∈ T u(·) X , the Jacobian is computed as where Φ(t, s) denotes the fundamental matrix of (5), satisfying the evolution equation Assuming that the Jacobian (7) is known, for a prescribed operational velocity w ∈ R r , we shall solve the Jacobian equation This equation is solvable for any w, provided that the Jacobian map is surjective, i.e. the endogenous configuration u(·) is regular (non-singular). Assuming regularity of u(·), a solution of (8) may be obtained by means of a right Jacobian inverse so that J q 0 ,T (u(·))J # q 0 ,T (u(·)) = I r . For a non-holonomic robotic system subject to constraints (1), the dynamics equations can be derived using the Lagrangian formalism and d'Alembert's principle. Let K (q,q) = 1 2q T M(q)q denote the kinetic energy of the unconstrained system, with the inertia matrix M(q) = M T (q) > 0. In the presence of nonholonomic constraints, this inertia matrix transforms to The matrix F(q) is symmetric and, for M(q) being positive definite and G(q) of full rank, F(q) is also positive definite.

Dynamically consistent Jacobian inverse for holonomic systems
The idea of the dynamically consistent Jacobian inverse is based on an assumption that the transmission of forces by the dual Jacobian inverse from the configuration to the operational space should be consistent with the system's dynamics in the operational space.
In the light of this assumption, Khatib's approach can be reconstructed in the following way.
Suppose that there are no non-holonomic constraints (G(q) = I n ), so the system's kinematics are described in coordinates by y = k(q), and its inertia matrix is M(q). It will be assumed that the kinematics are right invertible, so there exists a map l : be any right inverse of J (q). Having fixed this inverse, the map l(y) can be computed by a Jacobian inverse kinematics algorithm derived, e.g. on the basis of the continuation method. This derivation can be sketched as follows. Let q 0 denote an initial configuration, and q(θ ) ∈ R n , θ ∈ R, be a smooth curve starting from q(0) = q 0 . Given a desired point y in the operational space, we define an error e(θ ) = k(q(θ )) − y and request that along the curve q(θ ) the error satisfies a differential equation that makes the error decay exponentially with a decay rate γ > 0. After substituting for the error, we get By using a right inverse J # (q) of the Jacobian, this equation is converted into the explicit differential equation An often used right Jacobian inverse is the Jacobian pseudoinverse The inertia matrix M(q) defines a Riemannian met- This metric induces a pair of "musical isomorphisms" between the tangent and the cotangent space at q: the flat map Suppose also that there exists a Riemannian metric A specific form of g Y will be discovered later on. The elements p ∈ T * q Q and r ∈ T * y Y of cotangent spaces will be referred to as momenta and treated as covectors. Observe that the flat map results from the Legendre transformation that serves as a bridge between Lagrangian and Hamiltonian formulations of mechanics. In what follows we shall use the Riemannian metrics in order to present three methods of reconstruction of the dynamically consistent Jacobian inverse for holonomic systems.

Geometric method
By definition, the dual map to the Jacobian, J * (q) : where pv and r w denote the pairings between covectors and vectors. All these maps can be arranged into the following diagram: An inverse J # (q) that makes this diagram commutative for a suitably defined g Y (y) is called a dynamically consistent Jacobian inverse J # DC (q). Specifically, from commutativity of the lower left part of the diagram, it follows that After multiplying the above identity from the left by J (q), we conclude that In this way, the dynamically consistent Jacobian inverse is described by the matrix The matrix D C D (q) = J (q)M −1 (q)J T (q) can be called a dynamically consistent dexterity matrix. Configurations q at which rank D C D (q) is full are called regular, and the remaining configurations are singular. Observe that the obtained inverse does not satisfy the fourth Penrose equation [2]. It follows that the Riemannian metric on the operational space assumes the form Alternatively, the dynamic consistency means that the momenta that live in the null space of J # * (q) should not produce any motion in Y , so the corresponding velocity in T y Y needs to vanish. In the lower right part of the diagram (16), this is tantamount to the inclusion Since the null space of any dual right Jacobian inverse J # * (q) is spanned by id T * q Q − J * (q)J # * (q), in matrix form the identity (19) reads as which yields exactly the identity (17).

Force method
So far our exposition has been purely geometric, without any reference to forces acting in the holonomic system. In order to include these forces, suppose that the system's configuration moves along a smooth curve q(t) ∈ Q, with instantaneous velocityq(t) and momentum p(t) =q T (t)M(q(t)). The corresponding operational space trajectory is y(t) = k(q(t)), and the velocity in the operational space equalsẏ(t) = J (q(t))q(t). As the time derivative of the momentum, the force is also a covector and belongs to the cotangent space. Thus, we compute It is easily seen that for any right inverse of the Jacobian the cotangent space decomposes as therefore, in matrix terms, where Γ T is a force acting in the operational space, f T 0 acts in the configuration space. The acceleration in the operational space equals Putting all these things together, we arrive at the following equalitÿ In order that the force belonging to the null space of J # * (q) not affect the acceleration in the operational space, we need to have which, again, is equivalent to (17). The decomposition formula (21) with J # (q) replaced by J # DC (q) describes a decoupling of the configuration space forces into the forces coming from the operational space, and the internal forces that affect only the motion in the configuration space.

Optimization method
Last but not least, the dynamically consistent Jacobian inverse can be obtained directly as a solution of the following constrained optimization problem The Lagrange function that is tantamount to the expression (17).

Dynamically consistent Jacobian inverse for non-holonomic systems
Consider a non-holonomic system with the kinematics (2), the Jacobian (7), and the inertia matrix (10). As in case of the holonomic system, it will be assumed that there exists a right inverse L q 0 ,T : Y −→ X of the kinematics, such that K q 0 ,T • L q 0 ,T = id Y . For a y = K q 0 ,T (u(·)), let J # q 0 ,T : T y Y −→ T u(·) X denote a right inverse of the Jacobian. Given this right inverse, the map L q 0 ,T (y) can be computed by a Jacobian inverse kinematics algorithm obtained by using the continuation method along the lines analogous to those taken in Sect. 3. For completeness, below we shall briefly reproduce that reasoning. Beginning with an initial endogenous configuration u 0 (·) ∈ X , we make its deformation into a smooth curve u θ (·), where θ ∈ R. Next, for a desired point y in the operational space, we define the error e(θ ) = K q 0 ,T (u θ (·)) − y and require that along the curve u θ (·) the error decreases exponentially with a decay rate γ > 0, so that Using the error definition, we write Under the right inverse J # (u(·)) of the Jacobian, this equation is transformed into a functional differential equation The Jacobian pseudoinverse (the Moore-Penrose generalized inverse) is a typical right inverse of the Jacobian to be plugged into (23), see [22]. The dynamically consistent Jacobian inverse will be developed by analogy to the derivation for holonomic systems, sketched in Sect. 3. We begin with the introduction of a Riemannian metric into the endogenous configuration space. Let v 1 (·), v 2 (·) ∈ T u(·) X . We define where M q 0 (u(·))(t) = F(ϕ q 0 ,t (u(·))), F(q) is the inertia matrix (10), and q(t) = ϕ q 0 ,t (u(·)) denotes the trajectory of (2) subject to the control function u(·) ∈ X . Given this Riemannian metric, the musical isomorphisms can be defined in the following way where v(·) ∈ T u(·) X and p(·) ∈ T * u(·) X will be called, respectively, the endogenous velocity and momentum.
As in the case of the holonomic system, it is possible to define a sort of Legendre transform producing the flat isomorphism. The operational space will be equipped with a Riemannian metric g Y (y), to be specified further on. After these preparations, the dynamically consistent Jacobian inverse can be introduced in three ways.

Geometric method
For the Jacobian (7) and its right inverse J # q 0 ,T (u(·)), we consider a pair of dual maps J * q 0 ,T (u(·)) : T * y Y −→ T * u(·) X and J # * q 0 ,T (u(·)) : T * u(·) X −→ T * y Y , that operate in a standard way, namely and It follows from (27) that the dual Jacobian is determined by a matrix function (27) can be formulated in terms of the inner product in the endogenous configuration space (J * q 0 ,T (u(·))r )v(·) =<J q 0 ,T (u(·))(·)r T , v(·) > . A right Jacobian inverse will be referred to as dynamically consistent, if the following diagram of maps commutes.
Finally, using (26), we get or, in a more operational matrix form, where w ∈ T y Y . The matrix may be referred to as a dynamically consistent dexterity (or mobility) matrix, as introduced in [26]. The endogenous configuration u(·) for which rank D DC q 0 ,T (u(·)) = r is called regular; otherwise, this configuration is singular. It follows that the Riemannian metric in the operational space should be defined as Alternatively, making use of the lower right subdiagram of (29), the dynamic consistency requires that endogenous momenta belonging to the null space of J # * q 0 ,T (u(·)) should not produce any motion in Y , i.e.
The endogenous force will be treated as an element of the cotangent space T * u(·) X and can be computed as For any right Jacobian inverse, given the dual Jacobian and the dual Jacobian inverse, we have obviously therefore T * u(·) X = Im J * q 0 ,T (u(·))J # * q 0 ,T (u(·)) + Ker J # * q 0 ,T (u(·)) ⊂ Im J * q 0 ,T (u(·)) + Ker J # * q 0 ,T (u(·)). This means that T * u(·) X = Im J * q 0 ,T (u(·)) + Ker J # * q 0 ,T (u(·)). Furthermore, if there exists p(·) = J * q 0 ,T (u(·))r ∈ Ker J # * q 0 ,T (u(·)), then, after multiplying this equality from the left by J # * q 0 ,T (u(·)), we get r = 0, which produces the following decomposition of the cotangent space at u(·) T (u(·)). This decomposition allows us to represent the endogenous force f θ (·) as a sum where Γ T acts in the operational space, whereas the endogenous force f T 0 (·) is exerted in the endogenous configuration space. Computing from (36) the acceleration of the curve y θ in the operational space we can compose the above identities into the following acceleration formula To guarantee that the term containing f 0 (·) will not affect the acceleration in the operational space, we need to require that which implies that the Jacobian inverse should be dynamically consistent (30). As in the holonomic case, after the substitution of J # DC q 0 ,T (u(·)) for J # q 0 ,T (u(·)), the decomposition formula (38) establishes a decoupling of the endogenous forces into the forces coming from the operational space, and the internal forces that affect only the motion in the endogenous configuration space.

Optimization method
Analogously to the holonomic case, the dynamically consistent Jacobian inverse for the non-holonomic system can be obtained as a solution to the following constrained optimization problem under the equality constraint Invoking the standard methods of the calculus of variations, we introduce the Lagrange function where λ ∈ R r stands for a vector of Lagrange multipliers, and equate its derivative with respect to v(·) to 0. After the elimination of λ this yields with G DC q 0 ,T (u(·)) defined by (32), which is equivalent to (31). It is easily checked that the minimum value of the objective function is equal to w T G DC q 0 ,T −1 (u(·))w.

Holonomic systems
In Sect. 3.2, we have derived an operational space acceleration formula (22). Suppose that the dynamically consistent inverse (17) is used in this formula as the Jacobian inverse. Our aim will be to find an operational space force Γ that immobilizes the end effector. It is easily checked that this will be achieved, if Whenẏ(0) = 0, the force (40) will guarantee that y(t) = y(0) = const, independently of f 0 . An effect of application of this force in the configuration space, thanks to the identities (20) and (21), results in Now we takeq(0) = 0 (this forcesẏ(0) = 0), and solve (41) for q(t). By the presence of f 0 , the joints will move, while the end effector coordinates, y(t) = k(q(t)) = y(0), remain fixed. This will not be the case, if instead of the dynamically consistent inverse another Jacobian right inverse is employed. Illustrative computations are made using a planar 3R manipulator shown in Fig. 1. For where c i , c i j denote, respectively, cos q i and cos(q i + q j ). Computations have been run for f 0 = −1/100 (0, −9.81, 0) and the initial conditions q 0 = (0, π/3, 0), q 0 = (0, 0, 0), for T = 10. Results are presented in Fig. 2. The end effector steered by the dynamically consistent Jacobian inverse remains at the same place, despite the joints are moving. Also, the motion under J # DC shows much more "grace", if can be said so, than when the Jacobian pseudoinverse J # P (13) is applied.

Non-holonomic systems
By analogy to the holonomic case, we begin with the acceleration expression (39) Under assumption that the dynamically consistent Jacobian inverse (31) has been employed, an operational space force able to immobilize the system (i.e. to get identical output y θ (T ) for any θ ) can be computed as It follows that, if dy θ dθ | θ=0 = 0 then under the action of (42) the operational space curve y θ = y 0 = const, for any f 0 (·). To conclude, by combining (42) with (37) and (38) we arrive at a differential equation in the endogenous configuration space Having solved this equation for u θ (·) with initial condition du θ (·) dθ | θ=0 = 0 (or any other implying dy θ dθ | θ=0 = 0), we find the operational space curve y θ = K q 0 ,T (u θ (·)). When the dynamically consistent Jacobian inverse is used, this curve stays constant, y θ = y 0 = K q 0 ,T (u 0 (·)); otherwise, it does not. For computational reasons, since the Eq. (43) is defined in the infinite-dimensional endogenous configuration space, we need to introduce a finite-dimensional parametrization of control functions, e.g. using an orthogonal basis of functions in X . Let the matrix P(t) contain the basic functions arranged in such a way that  where s denotes the dimension of the parametrization. Having replaced u(·) by λ, we compute the parametrized trajectory of the system (2) and the endpoint map, ϕ q 0 ,t (λ) = ϕ q 0 ,t (u(·)),K q 0 ,T (λ) = K q 0 ,T (u(·)), as well as the Jacobian and the inertia matrix where A λ (t), B λ (t), C λ (t) should be computed for the parametrized control in accordance with (6). Consequently, the parametrized Riemannian metric where μ 1 , μ 2 ∈ R s , and This Riemannian metric generates the musical isomorphisms Finally, in accordance with the optimization method, we minimize the quadratic form μ T R q 0 ,T (λ)μ on condition thatJ q 0 ,T (λ)μ = w and obtain the parametrized dynamically consistent Jacobian inverse in the form wherẽ D DC q 0 ,T (λ) =J q 0 ,T (λ)R −1 q 0 ,T (λ)J T q 0 ,T (λ). Suppose that λ θ is a smooth curve in the parameter space R s , producing a curve u θ (t) = P(t)λ θ in the endogenous configuration space X . By analogy with subsection 4.2, we introduce for the curve λ θ the parametrized operational space curvẽ y θ =K q 0 ,T (λ θ ), its derivatives  Figure 6 shows the solutions of this problem provided by the dynamically consistent Jacobian inverse and the Jacobian pseudoinverse. The computations have been run until the error in the operational space drops below 10 −4 . The knot in the loop appearing on the path produced by J # P q 0 ,T corresponds to the rolling through the north pole (q 4 = θ = 0) that is a representation singularity of the ball, where the spherical coordinates as well the inertia matrix become singular. The introduction of J # DC q 0 ,T avoids this kind of motion.

Conclusion
Within the framework of the endogenous configuration space approach, we have extended the concept of the dynamically consistent Jacobian inverse from holonomic to non-holonomic robotic systems. This new inverse has been built around a Riemannian metric in the configuration space and founded on a conceptual analogy between holonomic and non-holonomic systems. The dynamically consistent Jacobian inverse has been applied to solve a motion planning problem for the rolling ball. Further research needs to be conducted towards using this inverse to the force control problems in non-holonomic systems.