# Gauss' principle and the dynamics of redundant and constrained manipulators

Herman Bruyninckx, Oussama Khatib

► To cite this version:

Herman Bruyninckx, Oussama Khatib. Gauss' principle and the dynamics of redundant and con- strained manipulators. IEEE International Conference on Robotics & Automation, Apr 2000, San Fransisco, CA, United States. pp.2563 - 2568, 10.1109/ROBOT.2000.846414 . hal-01375340

HAL Id: hal-01375340

https://hal.science/hal-01375340

Submitted on 3 Oct 2016

HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.

<figure><img src="image_1.png" /></figure>

Distributed under a Creative Commons Attribution 4.0 International License
# Gauss' Principle and the Dynamics of Redundant and Constrained Manipulators

Herman Bruyninckx
Katholieke Universiteit Leuven
Department of Mechanical Engineering
B-3001 Leuven, Belgium
herman.bruyninckx@mech.kuleuven.ac.be

Oussama Khatib
Stanford University
Dept. Computer Science, Robotics Lab
CA 94305, U.S.A.
khatib@cs.stanford.edu

## Abstract

This paper uses Gauss' Principle of Least Constraint to derive the "natural" dynamic equations for redundant manipulators. This approach is the fastest way to the result that the operational space inertia matrix of the manipulator is the natural weighting matrix for the projection used in solving the redundancy problem. Force-controlled robots form a special case of redundant robots, such that the results can be applied straightforwardly to solve the long-standing problem of the "non-invariance" of the Selection Matrices in the hybrid force/position control paradigm.

## 1 Introduction

Redundant robots have been studied extensively over the last three decades, starting with seminal papers such as [18] and [29]. From the start, most authors approached the problem on a kinematic level, i.e., using the velocities of the joints and the end-effector as variables, with the Jacobian matrix and its "generalized inverses" as basic mathematical concepts. A minority of papers considered the acceleration as more fundamental (e.g., [13, 14]), using the inertia properties of the manipulator in the generalized inverse. However, the problem of drift (or non-holonomicity) had soon been recognized, [16]. "Drift" shows up in general redundancy resolution algorithms when the robot repetitively performs the same path in Cartesian space, and the corresponding

joint space trajectories turn out to be non-repetitive. Since that time, most research concentrated fully on the velocity level, one of the reasons being that there, integrable, drift-free solutions are quite straightforward to implement, e.g., [2, 17, 23].

As mentioned above, the generalized inverse of the Jacobian matrix is the basic tool for all velocity-based techniques. Implicitly or explicitly, these generalized inverses rely on "weighting matrices," that are arbitrarily chosen by the user in order to achieve some optimality criterions; for example, minimization of joint torques or joint velocities, integrability, minimization of kinetic energy, minimization of distances to obstacles, etc. One important observation is hardly ever made: nature doesn't make an arbitrary choice. That means that, if one leaves a redundant system free, it will move in one specific, uniquely defined way, determined by how gravity interacts with its dynamic properties, [15]. ("Free" means: the joints are not actuated.) Similarly, for an actuated redundant robot, the trajectories generated by velocity-based redundancy resolution techniques are, in general, not those that the robot can execute "naturally," i.e., with a minimal dynamic effort. This reasoning leads automatically to the redundancy resolution approaches that consider the minimization of kinetic energy as the optimization criterion, e.g., [13, 14].

However, the kinematics community has never considered this criterion as a natural choice. The dynamics community has used, in different contexts, the inertia matrix of the manipulator as a "weighting matrix" because (i) it enters naturally in the mathematical modeling of vibrations of flexible manipulators,
e.g., [6]; (ii) it is the natural choice to decouple the internal motion dynamics from the end-effector dynamics for redundant manipulators, [15]; (iii) it enters naturally in the solution of the set of DAE ("differential algebraic equations") produced by, on the one hand, the dynamic equations of the robot, and, on the other hand, the algebraic constraint equations, e.g. [22, 27]. And the same weighting has already been used in the context of hybrid control before, [10], without extensive motivation.

The goal of this paper is to show the reader what is (probably) the shortest way to derive, from physical "first principles," the result that the generalized inverse weighted by the inertia matrix of the manipulator is the natural way to solve the redundancy problem. The "first principles" used in this derivation are: (i) Newton's law (and its 6D extension, the Newton-Euler equations), and (ii) Gauss' Principle of Least Constraint, [11]. This solution has been called the dynamically consistent inverse, e.g. [15].

A six degree of freedom robot in contact with the environment can be considered as a special case of a redundant robot. For example, in order to move the robot over a smooth surface with which it has a vertex-face contact, only three of the six degrees of freedom are needed. (Unless the desired orientation of the robot is specified.) Hence, it should come as no surprise that the same "dynamically natural" solution as in the redundancy problem case shows up in the problem of *hybrid control*. Hybrid control, [26], treats the contact between robot and environment as an infinitely stiff, "geometric" constraint, which forces the robot to move on a lower-than-six-dimensional manifold. This manifold can (in principle) be found by eliminating a number of variables equal to the number of constraint equations. The dynamically consistent generalized inverse is the natural way to "project" operational space velocities onto this submanifold. The corresponding "projection matrices" (often called "selection matrices") have had a long and controversial history in the area of force control, e.g., [1, 8, 20]. Nevertheless, some early papers (e.g., [22]) explained already basically the same material as what this paper presents, but the results were much more "hidden" in long mathematical calculations, and were not derived from "first principles." As

in the case of redundant manipulators, research in hybrid control theory was almost completely focused on kinematic approaches. Its current state of affairs is such that most researchers are not aware of the concepts and conclusions that this paper presents; hence, we think it is worthwhile to try to bring the message in its "purest" form.

**Overview of the paper.** Section 2 explains Gauss' Principle, which complements Newton's law of motion for constrained systems. (The paper doesn't discuss Newton's Law, or the Newton-Euler dynamics of unconstrained manipulators.) Sections 3. and 4 then apply it to the cases of redundant manipulators and force-controlled robots in the hybrid control paradigm.

## 2 Gauss' Principle

Gauss' Principle [11] is discussed in only a few (and mostly not so recent) dynamics articles or textbooks, [12, 25, 30]; two notable exceptions are [3] and [28]. Section 2.1 explains Gauss' Principle for the motion of a constrained point mass; Section 2.2 extends the discussion to a constrained rigid body. The constraints can be *physical* (imposed by nature, such as contacts or mechanical joint constraints) or *artificial* (i.e., abstract criterions specified by the user).

### 2.1 Gauss' Principle for a point mass

Together with its "cousins" d'Alembert's and Jourdain's Principles, Gauss' Principle is a basic axiom of physics, at the same level as Newton's laws of motion: the latter describe how masses move under the influence of forces, the Principles describe how to take geometric motion constraints into account. The Principles themselves cannot be derived from Newton's law, however. The following paragraphs state Gauss' Principle in both its "orthogonality" and minimization forms, [3, p. 314]:

**Orthogonality form** The ideal constraint forces do no "work" against the allowed accelerations $\dot{v}_i^a$:

$$ \sum_{i=1}^{N} (f_i - m_i \dot{v}_i) \cdot \dot{v}_i^a = 0. \quad (1) $$
$\dot{v}_i$ is the acceleration that the point mass $m_i$ will execute under the constraints when driven by a force $f_i$.

**Minimization form** The acceleration $\dot{v}_i$ minimizes the following “energy”:

$$E = \sum_{i=1}^{N} \frac{1}{2m_i} (f_i - m_i \dot{v}_i) \cdot (f_i - m_i \dot{v}_i). \quad (2)$$

Figure 1 depicts the situation for one single point mass. The concepts “work” and “energy” do not have their classical meaning, since there is an extra time derivative involved.

<figure><img src="image_2.png" /><figcaption>Figure 1: Constrained point mass. $v$ is its current velocity, $f$ is the applied force; $r$ is the radius of curvature of the constraining surface at the current position of the point.</figcaption></figure>

<figure><img src="image_3.png" /><figcaption>Figure 2: Constrained rigid body. $f$ and $m$ are the force and moments acting on the body. $v$ and $\omega$ are its current linear and angular velocities; they are such that the instantaneous velocity of each contact point, $v_c$ is tangential to the constraint.</figcaption></figure>

## 2.2 Gauss' Principle for a rigid body

Gauss' Principle for a constrained rigid body (Fig. 2) follows straightforwardly from Gauss' Principle for a set of point masses, [19] and [3, p. 474].

### Orthogonality form

$$(F - m\ddot{r}) \cdot \ddot{r} + (\tau - \dot{H}) \cdot \dot{\omega} = 0. \quad (3)$$

$\omega$ is the angular velocity of the rigid body, and $\dot{H} = \partial H / \partial t + \omega \times H$ is the total time derivative of the angular momentum. $(\ddot{r}, \dot{\omega})$ is any acceleration of the rigid body that is compatible with the constraints, i.e., the acceleration of each point of the moving rigid body that is in contact with the environment maintains the contact.

**Minimization form** It is well known (e.g., [9]) that a 6 × 6 so-called generalized mass matrix (or operational space inertia matrix) $M$ represents the linear and angular components of the rigid body's inertia. At the center of mass, its coordinate representation is simply a block-diagonal matrix with $m_{13 \times 3}$ and $I$ (the angular inertia) on the diagonal; but a general coordinate transformation of $M$ is known to involve a congruence transformation, which results in non-zero off-diagonal blocks. This generalized mass matrix allows to write the “acceleration energy” to be minimised in Gauss' Principle as

$$E = \frac{1}{2} (\mathcal{F} - M\mathbf{a})^T M^{-1} (\mathcal{F} - M\mathbf{a}). \quad (4)$$

$\mathcal{F}$ is the six-dimensional vector containing the concatenation of the three-dimensional force $F$ and moment $\tau$ used in Eq. (3). $\mathbf{a}$ is any constraint-compatible, six-dimensional acceleration vector $(\ddot{r}, \dot{\omega})$ of the rigid body. The minimization of the “energy” in Eq. (4) takes place over all possible accelerations $\mathbf{a}$, while the current positions and velocities are given. The motion constraints give rise to a linear constraint on the accelerations, and the following minimization procedure results:

$$\begin{gathered} \min_{\mathbf{a}} (\mathcal{F} - M\mathbf{a})^T M^{-1} (\mathcal{F} - M\mathbf{a}), \quad (5) \\ \text{subject to } A\mathbf{a} = \mathbf{b}. \end{gathered}$$

The matrix $A$ and the vector $\mathbf{b}$ are determined by the geometry of the constraint. The solution to such a constrained minimization problem is well known, [4], and uses the so-called weighted generalized inverse:

$$\mathbf{a} = A_M^\dagger \mathbf{b} = M^{-1} A_M^T (A M^{-1} A_M^T)^{-1} \mathbf{b}. \quad (6)$$
A is a $n \times 6$ matrix, with $n < 6$; $\alpha$ is a $6 \times 1$ vector;
M is a $6 \times 6$ matrix.

### 3 Dynamically consistent redundancy resolution

This Section explains the consequences of Gauss' Principle to the case of a redundant manipulator. This case is an extension to the previous paragraphs, in the sense that a manipulator consists of *multiple* rigid bodies, whose motion is *constrained* by the joints between them. Typically, each joint constrains five degrees of freedom. Extending the reasoning to multiple rigid bodies requires no new concepts, and leads (after some tedious algebra) to the following simple result, [19]: the operational space matrix $M$ is replaced by the $n \times n$ joint space inertia matrix $M$, with $n > 6$ the number of joints in the manipulator; the operational space acceleration $\alpha$ is replaced by the joint acceleration vector $\ddot{q}$; the constraint matrices $A$ and $b$ follow from the time derivative of the Jacobian equation:

$$J\dot{q} = V_d \quad \rightarrow \quad A = J, b = -\dot{J}\dot{q} + \dot{V}_d, \quad (7)$$

with $J$ the $6 \times n$ Jacobian matrix, $q$ the $n \times 1$ vector of joint angles, and $V_d$ the $6 \times 1$ user-specified spatial velocity. Equation (6) gives the generalized inverse for an $n \times 6$ matrix $A$, but now $J$ is $6 \times n$, with $n > 6$. The analytic form of the generalized inverse changes into

$$J_M^\dagger = (J M^{-1} J^T)^{-1} J M^{-1}. \quad (8)$$

This result is formally equal to the well-known choice of minimizing the instantaneous kinetic energy as the criterion for the redundancy resolution. However, Eq. (8) has been derived from minimizing the acceleration energy. It is clear from looking at the physical dimensions of the components that $J_M^\dagger$ is an operator that can act on spatial accelerations as well as on spatial velocities.

Gauss' Principle can be proven to be equivalent to d'Alembert's Principle for holonomic ("driftless") constraints; for non-holonomic constraints, d'Alembert's Principle gives incorrect results, while Gauss' Principle remains physically meaningful.

The weighted generalized inverse in Eq. (8) is exactly the dynamically consistent inverse introduced by [14], as the unique operator that solves the redundancy without injecting energy in the so-called "null space" components of the motion. The null space of the Jacobian $J$ are all joint velocities (or accelerations) that are mapped into a vanishing end-effector velocity (or acceleration). In other words, the motion of the end-effector is *dynamically decoupled* from the null space motion.

### 4 Hybrid force control

Just as for the redundant manipulator problem, most of the research in hybrid control took place at the *kinematic* level, i.e., the motion specification and control uses the *instantaneous velocities* of the robot end-effector, and (quasi-static) forces measured by the wrist force/torque sensor. The relationship with redundant manipulators should be clear from the material of the previous Sections. This Section explains the situation in hybrid force control in some more detail.

Some theoretical force control papers used d'Alembert's Principle, [5] to take into account a motion constraint. This Principle is formally the same as in Eq. (1), but with the acceleration $\dot{v}$ replaced by an infinitesimal displacement. Other popular formulations of d'Alembert's Principle are: "the constraint forces do no work," or "the instantaneous velocity is reciprocal, [24], to the space of the ideal constraint forces." This interpretation in terms of the reciprocity concept is popular amongst screw theory kinematics: even though Eq. (1) suggests a dynamic interpretation, it can be given a purely kinetostatic interpretation because one can replace the term $F_i - m_i a_i$ by the ith constraint force.

d'Alembert's Principle was (intuitively, not explicitly) rephrased in [21] and [26], as follows: for each motion constraint, there exists an orthogonal reference frame, called the *Compliance Frame*, that can be attached to the robot end-effector by the user in such a way that $n$ of its frame axes are velocity controlled (spanning the lower-than-six-dimensional subspace mentioned in the Introduction), and the other $6-n$ are force controlled. These directions were
called "orthogonal," and a "projection" on the force- and velocity-constrained directions was performed by selecting the components of the force and velocity vectors along the force- and velocity-controlled directions. This projection was represented by so-called *Selection Matrices*, whose form is particularly simple when expressed in the Compliance Frame: both are basically zero matrices, with only ones on their diagonals at places corresponding to the velocity or force-controlled directions.

These Selection Matrices were successfully used in many practical implementations. However, [8, 20] were the first to point out the theoretical inconsistencies of this approach: selecting a subset of *n* components from a six-dimensional vector of force or velocity is a *non-invariant* way to implement projection. "Non-invariant" means that this procedure yields different physical results when applied to the same forces and velocities, expressed in different reference frames.

Some people then realized that the projection ("Selection") needed in the hybrid control framework should be done by means of *weighted generalized inverses*, e.g., [7]. The generalized inverse of Eq. (8) is used to construct a *projection operator P* as follows:

$$P = JJ^{\dagger}_{M} \qquad (9)$$

It is easy to see that *P* is a real projection operator, in the sense that $PP = P$. *P* projects any Cartesian spatial velocity onto the subspace spanned by the columns of *J*. In the case of redundant manipulators, *J* was spanned by the spatial velocities generated by each of its joints; in the case of hybrid control, the columns of *J* are the degrees of freedom allowed by the contact. The projection *P* is not "orthogonal": the matrix *M* is used as weighting matrix.

## 5 Conclusions

This paper proves that the inertia matrix of a redundant or constrained manipulator is the dynamically *natural* weighting matrix for the generalized inverses that are used in every redundancy resolution algorithm, as well as (implicitly) in hybrid force control. The intuitive understanding of the theoretical background is remarkably simple, since the reader must

understand only two first principles: (i) Newton's law of motion for unconstrained systems, and (ii) Gauss' Principle of Least Constraint for constrained systems.

**Acknowledgment** H. Bruyninckx is Postdoctoral Fellow of the Fund for Scientific Research-Flanders (F.W.O.) in Belgium. A travel grant from F.W.O., financial support by the Belgian Programme on Inter-University Attraction Poles initiated by the Belgian State—Prime Minister's Office—Science Policy Programme (IUAP), and by K.U.Leuven's Concerted Research Action GOA/99/04 are gratefully acknowledged.

## References

[1] A. Abbati-Mariscotti, C. Bonivento, and C. Melchiorri. On the invariance of the hybrid position/force control. *J. Intelligent and Robotic Systems*, 3:233-250, 1990.

[2] J. Baillieul. Kinematic programming alternatives for redundant manipulators. In *IEEE Int. Conf. Robotics and Automation*, pages 722-728, St. Louis, MS, 1985.

[3] H. Baruh. *Analytical Dynamics*. WCB McGraw-Hill, 1999.

[4] A. Ben-Israel and T. N. E. Greville. *Generalized Inverses: Theory and Applications*. Robert E. Krieger Publishing Company, Huntington, NY, reprinted edition, 1980.

[5] J. L. R. D'Alembert. *Traité de Dynamique*. 1742.

[6] J. De Schutter, D. Torfs, S. Dutré, and H. Bruyninckx. Invariant hybrid position/force control of a velocity controlled robot with compliant end effector using modal decoupling. *Int. J. Robotics Research*, 16(3):340-356, 1997.

[7] K. L. Doty, C. Melchiorri, and C. Bonivento. A theory of generalized inverses applied to robotics. *Int. J. Robotics Research*, 12(1):1-19, 1993.

[8] J. Duffy. The fallacy of modern hybrid control theory that is based on "orthogonal complements" of twist and wrench spaces. *J. Robotic Systems*, 7(2):139-144, 1990.

[9] R. Featherstone. *Robot dynamics algorithms*. Kluwer, Boston, MA, 1987.
[10] R. Featherstone, S. Sonck, and O. Khatib. A general contact model for dynamically-decoupled force/motion control. In *IEEE Int. Conf. Robotics and Automation*, pages 3281-3286, Detroit, MI, 1999.

[11] K. F. Gauss. Über ein neues allgemeines Grundgesetz der Mechanik. *Journal für die Reine und Angewandte Mathematik*, 4:232–235, 1829.

[12] J. W. Gibbs. On the fundamental formulae of dynamics. *American Journal of Mathematics*, 2:49–64, 1879.

[13] K. Kazerounian and A. Nedungadi. Redundancy resolution of serial manipulators based on robot dynamics. *Mechanism and Machine Theory*, 23:295–303, 1988.

[14] O. Khatib. The augmented object and reduced effective inertia in robot systems. In *Proc. American Control Conference*, pages 2140–2147, Atlanta, GA, 1988.

[15] O. Khatib. Motion/force redundancy of manipulators. In *Japan-Usa Symposium on Flexible Automation*, pages 337–342, Kyoto, Japan, 1990.

[16] C. A. Klein and C. H. Huang. Review of pseudoinverse control for use with kinematically redundant manipulators. *IEEE Trans. Man Cybernetics*, 13, 1983.

[17] C. A. Klein and K.-B. Kee. The nature of drift in pseudoinverse control of kinematically redundant manipulators. *IEEE Trans. Rob. Automation*, 5(2):231–234, 1989.

[18] A. Liegeois. Automatic supervisory control of the configuration and behavior of multibody mechanisms. *IEEE Trans. on Systems, Man, and Cybernetics*, SMC-7(12):868–871, 1977.

[19] L. Lilov and M. Lorer. Dynamic analysis of multi-rigid body system based on the Gauss Principle. *Z. Angew. Math. Mechanik*, 62(10):539–545, 1982.

[20] H. Lipkin and J. Duffy. Hybrid twist and wrench control for a robotic manipulator. *Trans. ASME J. Mech. Transm. Automation Design*, 110:138–144, 1988.

[21] M. T. Mason. Compliance and force control for computer controlled manipulators. *IEEE Trans. on Systems, Man, and Cybernetics*, SMC-11(6):418–432, 1981.

[22] N. H. McClamroch. Singular systems of differential equations as dynamic models for constrained robot systems. In *IEEE Int. Conf. Robotics and Automation*, pages 21–28, San Francisco, CA, 1986.

[23] F. A. Mussa-Ivaldi and N. Hogan. Integrable solutions of kinematic redundancy via impedance control. *Int. J. Robotics Research*, 10(5):481–491, 1991.

[24] M. S. Ohwovoriole and B. Roth. An extension of screw theory. *Trans. ASME J. Mech. Design*, 103(4):725–735, 1981.

[25] L. A. Pars. *A treatise on analytical dynamics*. Heinemann, London, England, 1965.

[26] M. Raibert and J. J. Craig. Hybrid position/force control of manipulators. *Trans. ASME J. Dyn. Systems Meas. Control*, 102:126–133, 1981.

[27] N. Sadegh. Optimal kinematic control of constrained and redundant manipulators. In *31st Conf. Decision and Control*, pages 620–625, Tucson, AZ, 1992.

[28] F. E. Udwadia and R. E. Kalaba. *Analytical dynamics: a new approach*. Cambridge University Press, 1996.

[29] D. E. Whitney. Resolved motion rate control of manipulators and human prostheses. *IEEE Trans. Man-Machine Systems*, 10(2):47–53, 1969.

[30] J. Wittenburg. *Dynamics of systems of rigid bodies*. Teubner, Stuttgart, Germany, 1977.