Dynamically consistent Jacobian inverse for non-holonomic robotic systems

Abstract

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.

1 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 non-holonomic 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 non-holonomic systems. Section 5 demonstrates computational examples. The paper is concluded with Sect. 6.

2 Basic concepts

We shall study a robotic system described by generalized coordinates $q\in Q=\mathbf R ^n$, and velocities $\dot{q}\in T_qQ$, subject to $l<n$ independent phase constraints in the Pfaffian form

$$\begin{aligned} \mathrm {A}(q)\dot{q}=0, \end{aligned}$$

(1)

where $T_qQ\cong \mathbf R ^n$ denotes the tangent space to Q at q and $\mathrm {A}(q)$ is a constraints matrix of dimension $l\times 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

$$\begin{aligned} \left\{ \begin{array}{l} \dot{q}=G(q)u=\sum _{i=1}^mg_i(q)u_i,\\ y=k(q), \end{array} \right. \end{aligned}$$

(2)

where vector fields $g_i(q)$ span the null space of $\mathrm {A}(q)$, $u\in \mathbf R ^m$, $m=n-l$, is a control vector, and $y\in Y= \mathbf R ^r$ denotes operational coordinates. It will be assumed that admissible control functions of (2) belong to a linear subspace of the Lebesgue square integrable functions defined on [0, T] equipped with the inner product $<u_1(\cdot ),u_2(\cdot )> =\int _0^Tu^T_1(t)u_2(t)\mathrm{d}t$, and are such that for every the trajectory $q(t)=\varphi _{q_0,t}(u(\cdot ))$ 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 will be called the endogenous configuration space of the robotic system.

Let , where , be a Riemannian metric on the endogenous configuration space , and $g_Q(q):T_qQ\times T_qQ\longrightarrow \mathbf R$, $g_Y(y):T_yY\times T_yY\longrightarrow \mathbf R$, where $T_yY\cong \mathbf R ^r$, denote 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(\cdot )$,

$$\begin{aligned} K_{q_0,T}(u(\cdot ))=k(q(T))=k(\varphi _{q_0,T}(u(\cdot ))). \end{aligned}$$

(3)

The map (3) will be identified with the kinematics of (2). The derivative of the kinematics,

(4)

$T_{K_{q_0,T}(u(\cdot ))}Y\cong \mathbf R ^r$ being the tangent space to Y at the point $K_{q_0,T}(u(\cdot ))$, 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],

$$\begin{aligned} \left\{ \begin{array}{l} \dot{\xi }=A(t)\xi +B(t)v\\ w=C(t)\xi , \end{array}\right. \end{aligned}$$

(5)

initialized at $\xi _0=0$, where $\xi \in \mathbf R ^n$, $v\in \mathbf R ^m$, $w\in \mathbf R ^r$, and

$$\begin{aligned} A(t)=\frac{\partial G(q(t))u(t)}{\partial q},\; B(t)=G(q(t)),\; C(t)=\frac{\partial k(q(t))}{\partial q}. \end{aligned}$$

(6)

Given a control , the Jacobian is computed as

$$\begin{aligned} J_{q_0,T}(u(\cdot ))v(\cdot )=w(T)=C(T)\int _0^T\varPhi (T,t)B(t)v(t)\mathrm{d}t, \end{aligned}$$

(7)

where $\varPhi (t,s)$ denotes the fundamental matrix of (5), satisfying the evolution equation

$$\begin{aligned} \frac{\partial \varPhi (t,s)}{\partial t}=A(t)\varPhi (t,s),\quad \varPhi (s,s)=I_n. \end{aligned}$$

Assuming that the Jacobian (7) is known, for a prescribed operational velocity $w\in \mathbf R ^r$, we shall solve the Jacobian equation

$$\begin{aligned} J_{q_0,T}(u(\cdot ))v(\cdot )=w. \end{aligned}$$

(8)

This equation is solvable for any w, provided that the Jacobian map is surjective, i.e. the endogenous configuration $u(\cdot )$ is regular (non-singular). Assuming regularity of $u(\cdot )$, a solution of (8) may be obtained by means of a right Jacobian inverse

(9)

so that $J_{q_0,T}(u(\cdot ))J^{\#}_{q_0,T}(u(\cdot ))=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,\dot{q})=\frac{1}{2}\dot{q}^TM(q)\dot{q}$ denote the kinetic energy of the unconstrained system, with the inertia matrix $M(q)=M^T(q)>0$. In the presence of non-holonomic constraints, this inertia matrix transforms to

$$\begin{aligned} F(q)=G^T(q)M(q)G(q). \end{aligned}$$

(10)

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.

3 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:Y\longrightarrow Q$ such that the composition $k\circ l=id_{Y}$ equals the identity function on Y. Let $J(q)=\hbox {D}\,k(q):T_qQ\longrightarrow T_{k(q)}Y$ denote the Jacobian, and $J^{\#}(q):T_{y}Y\longrightarrow T_qQ$, $y=k(q)$, 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(\theta )\in \mathbf R ^n$, $\theta \in \mathbf 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(\theta )=k(q(\theta ))-y$ and request that along the curve $q(\theta )$ the error satisfies a differential equation

$$\begin{aligned} \frac{\mathrm{d} e(\theta )}{\mathrm{d}\theta }=-\gamma e(\theta ), \end{aligned}$$

(11)

that makes the error decay exponentially with a decay rate $\gamma >0$. After substituting for the error, we get

$$\begin{aligned} J(q)\frac{\mathrm{d} q(\theta )}{\mathrm{d}\theta }=-\gamma e(\theta ). \end{aligned}$$

By using a right inverse $J^{\#}(q)$ of the Jacobian, this equation is converted into the explicit differential equation

$$\begin{aligned} \frac{\mathrm{d} q(\theta )}{\mathrm{d}\theta }=-\gamma J^{\#}(q)(k(q)-y),\quad q(0)=q_0. \end{aligned}$$

(12)

An often used right Jacobian inverse is the Jacobian pseudoinverse

$$\begin{aligned} J^{\#\,P}(q)=J^T(q)\left( J(q)J^T(q)\right) ^{-1}. \end{aligned}$$

(13)

The inertia matrix M(q) defines a Riemannian metric $g_Q(q):T_qQ\times T_qQ\longrightarrow \mathbf R$ on Q, given as $g_Q(q)(v_1,v_2)=v_1^TM(q)v_2$. This metric induces a pair of “musical isomorphisms” between the tangent and the cotangent space at q: the flat map $g_Q^\flat (q):T_qQ\longrightarrow T^*_qQ$, and the sharp map $g_Q^\sharp (q)=(g_Q^\flat )^{-1}(q):T^*_qQ\longrightarrow T_qQ$, defined as

$$\begin{aligned} \begin{array}{l} g_Q^\flat (q)(v)=g_Q(q)(v,\cdot )=v^TM(q)=p,\\ \quad \text {and}\quad g_Q^\sharp (q)(p)=M^{-1}(q)p^T=v. \end{array} \end{aligned}$$

(14)

Suppose also that there exists a Riemannian metric $g_Y(y):T_yY\times T_yY\longrightarrow \mathbf R$ with associated isomorphisms

$$\begin{aligned} g_Y^\flat (y)(w)=g_Y(y)(w,\cdot )=r,\quad \text {and}\quad g_Y^\sharp (y)(r)=w. \end{aligned}$$

(15)

A specific form of $g_Y$ will be discovered later on. The elements $p\in T^*_qQ$ and $r\in T^*_yY$ 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.

3.1 Geometric method

By definition, the dual map to the Jacobian, $J^*(q):T^*_{k(q)}Y\longrightarrow T^*_qQ$ and the dual map to the Jacobian inverse $J^{\#*}(q):T^*_qQ\longrightarrow T^*_{k(q)}Y$ are such that $(J^*(q)r)v=rJ(q)v$ and $(J^{\#*}(q)p)w=pJ^{\#}(q)w$, where pv and rw denote the pairings between covectors and vectors. All these maps can be arranged into the following diagram:

(16)

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

$$\begin{aligned} J^{\#DC}(q)= g_Q^\sharp (q)J^*(q)g_Y^\flat (y),\quad y=k(q). \end{aligned}$$

After multiplying the above identity from the left by J(q), we conclude that

$$\begin{aligned} g_Y^\flat (y)=\left( J(q)g_Q^\sharp (q)J^*(q)\right) ^{-1}. \end{aligned}$$

In this way, the dynamically consistent Jacobian inverse is described by the matrix

$$\begin{aligned} J^{\#DC}(q)=M^{-1}(q)J^T(q)\left( J(q)M^{-1}(q)J^T(q)\right) ^{-1},\quad q=l(y).\nonumber \\ \end{aligned}$$

(17)

The matrix $D^{CD}(q)=J(q)M^{-1}(q)J^T(q)$ can be called a dynamically consistent dexterity matrix. Configurations q at which $\hbox {rank}\,D^{CD}(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

$$\begin{aligned} g_Y(y)(w_1,w_2)=w_1^T\left( D^{CD}\right) ^{-1}(l(y))w_2. \end{aligned}$$

(18)

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_yY$ needs to vanish. In the lower right part of the diagram (16), this is tantamount to the inclusion

$$\begin{aligned} \hbox {Ker}J^{\# DC *}(q)\subset \hbox {Ker}J(q) g_Q^\sharp (q). \end{aligned}$$

(19)

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

$$\begin{aligned} J(q)M^{-1}(q)(I_n-J^T(q)J^{\# DC\,T}(q))=0, \end{aligned}$$

which yields exactly the identity (17).

3.2 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)\in Q$, with instantaneous velocity $\dot{q}(t)$ and momentum $p(t)= \dot{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 $\dot{y}(t)=J(q(t))\dot{q}(t)$. As the time derivative of the momentum, the force is also a covector and belongs to the cotangent space. Thus, we compute

$$\begin{aligned} f(t)=\dot{p}(t)=\ddot{q}^TM(q(t))+\dot{q}^T\frac{\mathrm{d} M(q(t))}{\mathrm{d} t}. \end{aligned}$$

(20)

It is easily seen that for any right inverse of the Jacobian the cotangent space decomposes as

$$\begin{aligned} T^*_{q}Q=\hbox {Im}J^*(q)\oplus \hbox {Ker}J^{\#*}(q), \end{aligned}$$

therefore, in matrix terms,

$$\begin{aligned} f^T=J^T(q)\varGamma +(I_n-J^T(q)J^{\#T}(q))f_0, \end{aligned}$$

(21)

where $\varGamma ^T$ is a force acting in the operational space, $f^T_0$ acts in the configuration space. The acceleration in the operational space equals

$$\begin{aligned} \ddot{y}(t)=J(q(t))\ddot{q}+\frac{\mathrm{d} J(q(t))}{\mathrm{d} t}\dot{q}. \end{aligned}$$

Putting all these things together, we arrive at the following equality

$$\begin{aligned} \ddot{y}(t)= & {} J(q(t))M^{-1}(q(t))\left( J^T(q(t))\varGamma \right. \nonumber \\&+\,(I_n-J^T(q(t))J^{\#T}(q(t)))f_0-\left. \frac{\mathrm{d} M(q(t))}{\mathrm{d} t}\dot{q}(t)\right) \nonumber \\&+\,\frac{\mathrm{d} J(q(t))}{\mathrm{d} t}\dot{q}. \end{aligned}$$

(22)

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

$$\begin{aligned} J(q)M^{-1}(q)(I_n-J^T(q)J^{\#T}(q))=0, \end{aligned}$$

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.

3.3 Optimization method

Last but not least, the dynamically consistent Jacobian inverse can be obtained directly as a solution of the following constrained optimization problem

$$\begin{aligned} \min _{v} v^TM(q)v \quad \text {on condition that}\quad J(q)v=w. \end{aligned}$$

The Lagrange function

$$\begin{aligned} L(v,\lambda )=v^TM(q)v+\lambda ^T(J(q)v-w), \end{aligned}$$

therefore $\frac{\partial L(v, \,\lambda )}{\partial v}=0$ implies

$$\begin{aligned} v=M^{-1}(q)J^T(q)(J(q)M^{-1}(q)J^T(q))^{-1}w, \end{aligned}$$

that is tantamount to the expression (17).

4 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 of the kinematics, such that $K_{q_0,T}\circ L_{q_0,T}=id_{Y}$. For a $y=K_{q_0,T}(u(\cdot ))$, let 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 , we make its deformation into a smooth curve $u_\theta (\cdot )$, where $\theta \in \mathbf R$. Next, for a desired point y in the operational space, we define the error $e(\theta )=K_{q_0,T}(u_\theta (\cdot ))-y$ and require that along the curve $u_\theta (\cdot )$ the error decreases exponentially with a decay rate $\gamma >0$, so that

$$\begin{aligned} \frac{\mathrm{d} e(\theta )}{\mathrm{d}\theta }=-\gamma e(\theta ). \end{aligned}$$

Using the error definition, we write

$$\begin{aligned} J_{q_0,T}(u_\theta (\cdot ))\frac{\mathrm{d} u_\theta (\cdot )}{\mathrm{d}\theta }=-\gamma e(\theta ). \end{aligned}$$

Under the right inverse $J^{\#}(u(\cdot ))$ of the Jacobian, this equation is transformed into a functional differential equation

$$\begin{aligned}&\frac{\mathrm{d} u_\theta (\cdot )}{\mathrm{d}\theta }=-\gamma J^{\#}(u_\theta (\cdot ))(K_{q_0,T}(u_\theta (\cdot ))-y),\nonumber \\&\quad u_{\theta =0}(\cdot )=u_0(\cdot ). \end{aligned}$$

(23)

The Jacobian pseudoinverse (the Moore–Penrose generalized inverse)

$$\begin{aligned}&(J^{\#P}_{q_0,T}(u(\cdot ))w)(t)=B^T(t)\varPhi ^T(T,t) C^T(T)\nonumber \\&\times \left( C(T)\left[ \int _0^T\varPhi (T,t)B(t)B^T(t) \varPhi ^T(T,t)\mathrm{d}t\right] C^T(T)\right) ^{-1}w,\nonumber \\ \end{aligned}$$

(24)

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 . We define

(25)

where ${\mathscr {M}}_{q_0}(u(\cdot ))(t)=F(\varphi _{q_0,t}(u(\cdot )))$, F(q) is the inertia matrix (10), and $q(t)=\varphi _{q_0,t}(u(\cdot ))$ denotes the trajectory of (2) subject to the control function . Given this Riemannian metric, the musical isomorphisms can be defined in the following way

(26)

where and 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.

4.1 Geometric method

For the Jacobian (7) and its right inverse $J^{\#}_{q_0,T}(u(\cdot ))$, we consider a pair of dual maps and , that operate in a standard way, namely

$$\begin{aligned} (J^*_{q_0,T}(u(\cdot ))r)v(\cdot )= & {} rJ_{q_0,T}(u(\cdot ))v(\cdot )r\nonumber \\= & {} C(T)\int _0^T\varPhi (T,t)B(t)v(t)\mathrm{d}t,\nonumber \\ \end{aligned}$$

(27)

and

$$\begin{aligned} (J^{\#*}_{q_0,T}(u(\cdot ))p(\cdot ))w=p(\cdot )J^{\#}_{q_0,T}(u(\cdot ))w. \end{aligned}$$

(28)

It follows from (27) that the dual Jacobian is determined by a matrix function

$$\begin{aligned} \bar{J}^T_{q_0,T}(u(\cdot ))(t)=B^T(t)\varPhi ^T(T,t)C^T(T), \end{aligned}$$

such that (27) can be formulated in terms of the inner product in the endogenous configuration space

$$\begin{aligned} (J^*_{q_0,T}(u(\cdot ))r)v(\cdot )=<\bar{J}_{q_0,T}(u(\cdot ))(\cdot )r^T,v(\cdot )>. \end{aligned}$$

A right Jacobian inverse will be referred to as dynamically consistent, if the following diagram of maps commutes.

(29)

Requiring commutativity of the lower left subdiagram, we obtain

A composition from the left of the above identity with $J_{q_0,T}(u(\cdot ))$ results in

Finally, using (26), we get

(30)

or, in a more operational matrix form,

$$\begin{aligned}&(J^{\#DC}_{q_0,T}(u(\cdot ))w)(t)\nonumber \\&\quad ={\mathscr {M}}_{q_0}^{-1}(u(\cdot ))(t)B^T(t)\varPhi ^T(T,t) C^T(T)\left( {\mathscr {D}}_{q_0,T}^{DC}\right) ^{-1}(u(\cdot ))w,\nonumber \\ \end{aligned}$$

(31)

where $w\in T_yY$. The matrix

$$\begin{aligned}&{\mathscr {D}}_{q_0,T}^{DC}(u(\cdot )) =C(T)\nonumber \\&\quad \left[ \int _0^T\varPhi (T,t)B(t){\mathscr {M}}_{q_0}^{-1}(u(\cdot ))(t)B^T(t)\varPhi ^T(T,t)\mathrm{d}t\right] C^T(T)\nonumber \\ \end{aligned}$$

(32)

may be referred to as a dynamically consistent dexterity (or mobility) matrix, as introduced in [26]. The endogenous configuration $u(\cdot )$ for which $\hbox {rank}\,{\mathscr {D}}_{q_0,T}^{DC}(u(\cdot ))=r$ is called regular; otherwise, this configuration is singular. It follows that the Riemannian metric in the operational space should be defined as

$$\begin{aligned} g_Y(y)(w_1,w_2)=w_1^T\left( {\mathscr {D}}_{q_0,T}^{DC}\right) ^{-1} (L_{q_0,T}(y))w_2, \end{aligned}$$

(33)

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(\cdot ))$ should not produce any motion in Y, i.e.

(34)

Given a right Jacobian inverse $J^{\#}_{q_0,T}(u(\cdot ))$, the null space of $J^{\#*}_{q_0,T}(u(\cdot ))$ is spanned by . This being so, the identity (34) implies that

(35)

from which one directly extracts (30).

4.2 Force method

We shall proceed analogously as in Sect. 3.2. Let the endogenous configuration move in along a smooth curve $u_{\theta }(\cdot )$. Let the corresponding endogenous velocity be equal to $\frac{\mathrm{d} u_\theta (\cdot )}{\mathrm{d}\theta }$ and the endogenous momentum $p_\theta (\cdot )=\left( \frac{\mathrm{d} u_\theta (\cdot )}{\mathrm{d}\theta }\right) ^T{\mathscr {M}}_{q_0}(u_\theta (\cdot ))(\cdot )$. The endogenous configuration curve $u_\theta (\cdot )$ produces an operational space curve $y_\theta =K_{q_0,T}(u_\theta (\cdot ))$, and the velocity transformation is described by the Jacobian (7), i.e.

$$\begin{aligned} \frac{\mathrm{d} y_\theta }{\mathrm{d}\theta }=J_{q_0,T}(u_\theta (\cdot ))\frac{\mathrm{d} u_\theta (\cdot )}{\mathrm{d}\theta }. \end{aligned}$$

(36)

The endogenous force will be treated as an element of the cotangent space and can be computed as

$$\begin{aligned} f_\theta (\cdot )= & {} \frac{\mathrm{d} p_\theta (\cdot )}{\mathrm{d}\theta }=\left( \frac{\mathrm{d}^2 u_\theta (\cdot )}{\mathrm{d}\theta ^2}\right) ^T{\mathscr {M}}_{q_0}(u_\theta (\cdot ))(\cdot )\nonumber \\&+ \left( \frac{\mathrm{d} u_\theta (\cdot )}{\mathrm{d}\theta }\right) ^T\frac{d {\mathscr {M}}_{q_0}(u_\theta (\cdot ))(\cdot )}{\mathrm{d}\theta }. \end{aligned}$$

(37)

For any right Jacobian inverse, given the dual Jacobian and the dual Jacobian inverse, we have obviously

therefore

This means that

Furthermore, if there exists $p(\cdot )=J^*_{q_0,T}(u(\cdot ))r\in \hbox {Ker}J^{\#*}_{q_0,T}(u(\cdot ))$, then, after multiplying this equality from the left by $J^{\#*}_{q_0,T}(u(\cdot ))$, we get $r=0$, which produces the following decomposition of the cotangent space at $u(\cdot )$

This decomposition allows us to represent the endogenous force $f_\theta (\cdot )$ as a sum

(38)

where $\varGamma ^T$ acts in the operational space, whereas the endogenous force $f_0^T(\cdot )$ is exerted in the endogenous configuration space. Computing from (36) the acceleration of the curve $y_\theta$ in the operational space

$$\begin{aligned} \frac{\mathrm{d}^2 y_\theta }{\mathrm{d}\theta ^2}=J_{q_0,T}(u_\theta (\cdot ))\frac{\mathrm{d}^2 u_\theta (\cdot )}{\mathrm{d}\theta ^2}+\frac{\mathrm{d} J_{q_0,T}(u_\theta (\cdot ))}{\mathrm{d}\theta }\frac{\mathrm{d} u_\theta (\cdot )}{\mathrm{d}\theta }, \end{aligned}$$

we can compose the above identities into the following acceleration formula

(39)

To guarantee that the term containing $f_0(\cdot )$ 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_{q_0,T}^{\# DC}(u(\cdot ))$ for $J_{q_0,T}^{\#}(u(\cdot ))$, 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.

4.3 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

$$\begin{aligned} J_{q_0,T}(u(\cdot ))=C(T)\int _0^T\varPhi (T,t)B(t)v(t)\mathrm{d}t=w. \end{aligned}$$

Invoking the standard methods of the calculus of variations, we introduce the Lagrange function

$$\begin{aligned}&{\mathscr {L}}(v(\cdot ),\lambda )\\&=\int _0^T\left( v^T(t){\mathscr {M}}_{q_0}(u(\cdot ))(t)v(t)+ \lambda ^TC(T)\varPhi (T,t)B(t)v(t)\right) \mathrm{d}t, \end{aligned}$$

where $\lambda \in \mathbf R ^r$ stands for a vector of Lagrange multipliers, and equate its derivative with respect to $v(\cdot )$ to 0. After the elimination of $\lambda$ this yields

$$\begin{aligned}&v(t)=J^{\#\,DC}_{q_0,T}(u(\cdot ))w={\mathscr {M}}_{q_0}^{-1}(u(\cdot ))(t)B^T(t)\\&\times \,\varPhi ^T(T,t)C^T(T) ({\mathscr {G}}^{DC}_{q_0,T})^{-1}(u(\cdot ))w, \end{aligned}$$

with ${\mathscr {G}}^{DC}_{q_0,T}(u(\cdot ))$ 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\left( {\mathscr {G}}^{DC}_{q_0,T}\right) ^{-1}(u(\cdot ))w$.

5 Examples

5.1 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 $\varGamma$ that immobilizes the end effector. It is easily checked that this will be achieved, if

$$\begin{aligned} \varGamma =J^{\# DC\,T}(q)\frac{\mathrm{d} M(q(t))}{\mathrm{d} t}\dot{q}-\left( D^{DC}\right) ^{-1}(q)\frac{\mathrm{d} J(q(t))}{\mathrm{d} t}\dot{q}. \end{aligned}$$

(40)

When $\dot{y}(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

$$\begin{aligned}&M(q)\ddot{q}+\left( I_n-J^T(q)J^{\# DC\,T}(q)\right) \left( \frac{\mathrm{d} M(q(t))}{\mathrm{d} t}\dot{q}-f_0\right) \nonumber \\&\qquad +\,J^T(q)\left( D^{DC}\right) ^{-1}(q)\frac{\mathrm{d} J(q(t))}{\mathrm{d} t}\dot{q}=0. \end{aligned}$$

(41)

Now we take $\dot{q}(0)=0$ (this forces $\dot{y}(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.

Fig. 1

3R planar manipulator

Illustrative computations are made using a planar 3R manipulator shown in Fig. 1. For simplicity, the unit link lengths $l_1=l_2=l_3=1$ and masses $m_1=m_2=m_3=1$ are chosen. With these substitutions, the inertia matrix

$$\begin{aligned} M(q)=\left[ \begin{array}{ccc} \frac{1}{3}(12+3c_2+c_3+c_{23})&{} \frac{1}{6}(10+9c_ 2 +6c_3 +c_{23})&{} \frac{1}{6}(2 + 3c_3 +3c_{23})\\ \frac{1}{6}(10+9c_2 +6c_3 +c_{23})&{} 1+c_ 3 &{} \frac{1}{6}(2 + 3c_3)\\ \frac{1}{6}(2 + 3c_3 +3c_{23})&{} \frac{1}{6}(2 + 3c_3)&{} \frac{1}{3} \end{array}\right] , \end{aligned}$$

where $c_i$, $c_{ij}$ 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,\pi /3,0)$, $\dot{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.

Fig. 2

3R planar manipulator: joint trajectories and postures for $J^{\# DC}$ and $J^{\# P}$ Jacobian inverses

5.2 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_\theta (T)$ for any $\theta$) can be computed as

$$\begin{aligned} \varGamma= & {} J_{q_0,T}^{\# DC\,*}(u_{\theta }(\cdot ))\frac{\mathrm{d} {\mathscr {M}}_{q_0}(u_{\theta }(\cdot ))(\cdot )}{\mathrm{d} \theta }\frac{\mathrm{d} u_\theta (\cdot )}{\mathrm{d}\theta }\nonumber \\&-\left( {\mathscr {D}}_{q_0,T}^{DC}\right) ^{-1} (u_{\theta }(\cdot )) \frac{\mathrm{d} J_{q_0,T}(u_{\theta }(\cdot ))}{\mathrm{d} \theta }\frac{\mathrm{d} u_\theta (\cdot )}{\mathrm{d}\theta }. \end{aligned}$$

(42)

It follows that, if $\frac{d y_\theta }{d\theta }|_{\theta =0}=0$ then under the action of (42) the operational space curve $y_\theta =y_0=const$, for any $f_0(\cdot )$. To conclude, by combining (42) with (37) and (38) we arrive at a differential equation in the endogenous configuration space

(43)

Having solved this equation for $u_\theta (\cdot )$ with initial condition $\frac{\mathrm{d} u_\theta (\cdot )}{\mathrm{d}\theta }|_{\theta =0}=0$ (or any other implying $\frac{\mathrm{d} y_\theta }{\mathrm{d}\theta }|_{\theta =0}=0$), we find the operational space curve $y_\theta =K_{q_0,T}(u_\theta (\cdot ))$. When the dynamically consistent Jacobian inverse is used, this curve stays constant, $y_\theta =y_0=K_{q_0,T}(u_0(\cdot ))$; 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 . Let the matrix P(t) contain the basic functions arranged in such a way that

$$\begin{aligned} u(t)=P(t)\lambda ,\quad \lambda \in \mathbf R ^s, \quad \text {and}\quad \int _0^TP^T(t)P(t)\mathrm{d}t=I_s, \end{aligned}$$

where s denotes the dimension of the parametrization. Having replaced $u(\cdot )$ by $\lambda$, we compute the parametrized trajectory of the system (2) and the endpoint map,

$$\begin{aligned} \tilde{\varphi }_{q_0,t}(\lambda )=\varphi _{q_0,t}(u(\cdot )),\quad \tilde{K}_{q_0,T}(\lambda )=K_{q_0,T}(u(\cdot )), \end{aligned}$$

as well as the Jacobian and the inertia matrix

$$\begin{aligned}&\tilde{J}_{q_0,T}(\lambda )=C_\lambda (T)\int _0^T\varPhi _\lambda (T,t)B_\lambda (t)P(t)\mathrm{d}t, \\&\quad \tilde{{\mathscr {M}}}_{q_0}(\lambda )(t)=F(\tilde{\varphi }_{q_0,t}(\lambda )), \end{aligned}$$

where $A_\lambda (t)$, $B_\lambda (t)$, $C_\lambda (t)$ should be computed for the parametrized control in accordance with (6). Consequently, the parametrized Riemannian metric

where $\mu _1,\mu _2\in \mathbf R ^s$, and

$$\begin{aligned} {\mathscr {R}}_{q_0,T}(\lambda )=\int _0^TP^T(t)\tilde{{\mathscr {M}}}_{q_0}(\lambda )(t)P(t)\mathrm{d}t. \end{aligned}$$

This Riemannian metric generates the musical isomorphisms

Finally, in accordance with the optimization method, we minimize the quadratic form $\mu ^T{\mathscr {R}}_{q_0,T}(\lambda )\mu$ on condition that $\tilde{J}_{q_0,T}(\lambda )\mu =w$ and obtain the parametrized dynamically consistent Jacobian inverse in the form

$$\begin{aligned} \tilde{J}^{\# DC}_{q_0,T}(\lambda )={\mathscr {R}}^{-1}_{q_0,T}(\lambda )\tilde{J}^T_{q_0,T}(\lambda )\left( \tilde{{\mathscr {D}}}^{DC}_{q_0,T}(\lambda )\right) ^{-1}, \end{aligned}$$

where

$$\begin{aligned} \tilde{{\mathscr {D}}}^{DC}_{q_0,T}(\lambda )=\tilde{J}_{q_0,T}(\lambda ){\mathscr {R}}^{-1}_{q_0,T}(\lambda )\tilde{J}^T_{q_0,T}(\lambda ). \end{aligned}$$

Suppose that $\lambda _\theta$ is a smooth curve in the parameter space $\mathbf R ^s$, producing a curve $u_\theta (t)=P(t)\lambda _\theta$ in the endogenous configuration space . By analogy with subsection 4.2, we introduce for the curve $\lambda _\theta$ the parametrized operational space curve

$$\begin{aligned} \tilde{y}_\theta =\tilde{K}_{q_0,T}(\lambda _\theta ), \end{aligned}$$

its derivatives

$$\begin{aligned} \frac{\mathrm{d}\tilde{y}_\theta }{\mathrm{d}\theta }= & {} \tilde{J}_{q_0,T}(\lambda _\theta )\frac{\mathrm{d}\lambda _\theta }{\mathrm{d}\theta },\quad \frac{\mathrm{d}^2\lambda _\theta }{\mathrm{d}\theta ^2}=\tilde{J}_{q_0,T}(\lambda _\theta )\frac{\mathrm{d}^2\lambda _\theta }{\mathrm{d}\theta ^2}\\&+\frac{\mathrm{d}\tilde{J}_{q_0,T}(\lambda _\theta )}{\mathrm{d}\theta }\frac{\mathrm{d}\lambda _\theta }{\mathrm{d}\theta }, \end{aligned}$$

as well as the parametrized momentum

$$\begin{aligned} p_\theta =\left( \frac{\mathrm{d}\lambda _\theta }{\mathrm{d}\theta }\right) ^T{\mathscr {R}}_{q_0,T}(\lambda _\theta ), \end{aligned}$$

and the parametrized force

$$\begin{aligned} f_\theta ^T= & {} \frac{\mathrm{d} p_\theta }{\mathrm{d}\theta }=\left( \frac{\mathrm{d}^2\lambda _\theta }{\mathrm{d}\theta ^2}\right) ^T{\mathscr {R}}_{q_0,T}(\lambda _\theta )\nonumber \\&+\left( \frac{\mathrm{d}\lambda _\theta }{\mathrm{d}\theta }\right) ^T\frac{\mathrm{d}{\mathscr {R}}_{q_0,T}(\lambda _\theta )}{\mathrm{d}\theta }. \end{aligned}$$

(44)

Let $\tilde{J}^{\#}_{q_0,T}(\lambda )$ denote a right inverse of the parametrized Jacobian. Then, as in (38), we have a decomposition

$$\begin{aligned} f_\theta =\tilde{J}^T_{q_0,T}(\lambda _\theta )\varGamma +\left( I_s-\tilde{J}^T_{q_0,T}(\lambda _\theta )\tilde{J}^{\#\,T}_{q_0,T}(\lambda _\theta )\right) f_0. \end{aligned}$$

(45)

Using these outcomes, after suitable substitutions, we arrive at the following

$$\begin{aligned}&\frac{\mathrm{d}^2\tilde{y}_\theta }{\mathrm{d}\theta ^2}=\tilde{J}_{q_0,T}(\lambda _\theta ){\mathscr {R}}^{-1}_{q_0,T}(\lambda _\theta )\left( \tilde{J}^T_{q_0,T}(\lambda _\theta )\varGamma \right. \\&\left. +\left( I_s-\tilde{J}^T_{q_0,T}(\lambda _\theta )\tilde{J}^{\#\,T}_{q_0,T}(\lambda _\theta )\right) f_0\right. \\&\left. -\frac{\mathrm{d}{\mathscr {R}}_{q_0,T}(\lambda _\theta )}{\mathrm{d}\theta }\frac{\mathrm{d}\lambda _\theta }{\mathrm{d}\theta }\right) +\ \frac{\mathrm{d}\tilde{J}_{q_0,T}(\lambda _\theta )}{\mathrm{d}\theta }\frac{\mathrm{d}\lambda _\theta }{\mathrm{d}\theta }. \end{aligned}$$

In order to get $\tilde{y}_\theta =y_0=\tilde{K}_{q_0,T}(\lambda _0)$, we use the parametrized dynamically consistent Jacobian inverse, request that

$$\begin{aligned}&\varGamma =\tilde{J}^{\# DC\,T}_{q_0,T}(\lambda _\theta )\frac{\mathrm{d}{\mathscr {R}}_{q_0,T}(\lambda _\theta )}{\mathrm{d}\theta }\frac{\mathrm{d}\lambda _\theta }{\mathrm{d}\theta }\\&-\left( \tilde{{\mathscr {D}}}^{DC}_{q_0,T}\right) ^{-1} (\lambda _\theta ) \frac{\mathrm{d}\tilde{J}_{q_0,T}(\lambda _\theta )}{\mathrm{d}\theta }\frac{\mathrm{d}\lambda _\theta }{\mathrm{d}\theta }, \end{aligned}$$

and assume that $\frac{\mathrm{d}\lambda _\theta }{\mathrm{d}\theta }|_{\theta =0}=0$. In conclusion, a combination of (44) and (45) yields the following differential equation for $\lambda _\theta$

$$\begin{aligned}&{\mathscr {R}}_{q_0,T}(\lambda _\theta )\frac{\mathrm{d}^2\lambda _\theta }{\mathrm{d}\theta ^2}\nonumber \\&\quad +\, \left( I_s-\tilde{J}^T_{q_0,T}(\lambda _\theta )\tilde{J}^{\# DC\,T}_{q_0,T}(\lambda _\theta )\right) \left( \frac{\mathrm{d}{\mathscr {R}}_{q_0,T}(\lambda _\theta )}{\mathrm{d}\theta }\frac{\mathrm{d}\lambda _\theta }{\mathrm{d}\theta }-f_0\right) \nonumber \\&\quad +\, \tilde{J}^{T}_{q_0,T}(\lambda _\theta )\left( \tilde{{\mathscr {D}}}^{DC}_{q_0,T}\right) ^{-1}(\lambda _\theta ) \frac{\mathrm{d}\tilde{J}_{q_0,T}(\lambda _\theta )}{\mathrm{d}\theta }\frac{\mathrm{d}\lambda _\theta }{\mathrm{d}\theta }=0. \end{aligned}$$

(46)

After solving (46), we get $\tilde{y}_\theta =\tilde{K}_{q_0,T}(\lambda _\theta )$. If the parametrized dynamically consistent Jacobian inverse has been chosen, it should be $\tilde{y}_\theta =y_0=\tilde{K}_{q_0,T}(\lambda _0)$.

Fig. 3

Pioneer 2DX

We shall check this for a simplified, unicycle-type, model of the Pioneer 2DX mobile robot, moving without a lateral slip of the wheels, shown schematically in Fig. 3. The vector of generalized coordinates $q=(x,y,\theta )$. The robot’s mass $m=8.67$, the moment of inertia with respect to the Z-axis, $I=0.256$, the distance between driving wheels $2l=0.326$. The non-holonomic inertia matrix (10) is constant, $F(q)=diag\{m,\,I\}$. The control functions are chosen in the form of the trigonometric series with nine terms for every input. The initial state $q_0=(1,0,\pi /4)$, the control horizon $T=5$. The initial control is set to $\lambda _0=(1,0,0.5,0_{1\times 6},0.5,0_{1\times 8})\in \mathbf R ^{18}$. The internal force $f_0=(0,0,1,0_{1\times 8},0.1,0,1,0_{1\times 4})\in \mathbf R ^{18}$. Results of computations are shown in Fig. 4. Driven by the dynamically consistent Jacobian inverse the Pioneer for consecutive $\theta$ reaches the same final position and orientation, despite the existence of the force $f_0$. It can be noticed that this does not happen in case of the Jacobian pseudoinverse (24).

Fig. 4

Pioneer 2DX: controls and XY-paths for $J_{q_0,T}^{\# DC}$, and XY-paths for $J_{q_0,T}^{\# P}$ inverse (bottom right). The arrows indicate increase of $\theta$

Fig. 5

Rolling ball

Fig. 6

Kinematics of the ball: paths in the XY plane, trajectories of $q_1,q_2$ and $q_4,q_5$, velocities $\dot{q}_1$ and $\dot{q}_2$

In order to further demonstrate the performance of a non-holonomic system driven by the dynamically consistent Jacobian inverse, we shall solve an example motion planning problem for the rolling ball, using the dynamically consistent inverse $J_{q_0,T}^{\# DC}$ and the Jacobian pseudoinverse $J_{q_0,T}^{\# P}$. The ball is shown in Fig. 5. Its coordinate vector $q=(x,y,\varphi ,\theta ,\psi )$ consists of the Cartesian coordinates of the contact point with respect to the external frame, spherical coordinates of this point with respect to the ball frame, and the ball orientation angle. The ball has the unit mass $m=1$, the radius $R=0.1$, and the inertia $I=\frac{2}{5}mR^2$. The kinematics of the rolling ball are represented by the driftless control system (2) with the control matrix

$$\begin{aligned} G(q)=\begin{bmatrix} R s_4 s_5&R c_5\\ -Rs_4 c_5&R s_5\\ 1&0\\ 0&1\\ -c_4&0\end{bmatrix}, \end{aligned}$$

$s_i$, $c_j$ standing for $\sin q_i$ and $\cos q_j$, and the output function $k(q)=(q_1,q_2,q_5)$. The non-holonomic inertia matrix $F(q)=(I+mR^2)diag\{s^2_4,1\}$. The motion planning problem consists in the ball reaching the position and orientation $y_d=(1,0,-\frac{\pi }{2})$ in time $T=5$. The control functions are assumed trigonometric, containing up to third-order harmonics. The initial state of the ball is assumed $q_0=(0,0,0,\frac{\pi }{4},\frac{\pi }{2})$, the initial control coefficients $\lambda _0=(5,0_{1\times 6},0.1,0_{1\times 6})\in R^{14}$, and $\gamma =0.02.$ 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_{q_0,T}^{\# P}$ corresponds to the rolling through the north pole ($q_4=\theta =0$) that is a representation singularity of the ball, where the spherical coordinates as well the inertia matrix become singular. The introduction of $J_{q_0,T}^{\# DC}$ avoids this kind of motion.

6 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.