1 Introduction

The dynamics of both holonomically and non-holonomically constrained mechanical systems represents a significant chapter in the history of classical mechanics [21, 22]. Formulations addressing constraints have included the method of Lagrange multipliers [20], Gauss’ Principle of Least Constraint [12], Hertz’ Principle of Least Curvature (a special case of Gauss’ Principle) [14], and the Gibbs–Appell equations [3, 13], among others [26]. While the configuration space dynamics of constrained systems has been well researched and understood for some time, relevant research issues exist with regard to the control of constrained systems. In previous work, the control of constrained systems has been examined from a configuration space perspective [16]. This includes the use of projection based approaches [1, 2, 5]. Additionally, contact control in robot manipulators has also employed constraint-based approaches [15, 23, 24], again, from a configuration space perspective. However, it is of particular importance to formulate a control framework that is well-suited to the structure of constrained systems. Specifically, with respect to holonomic constraints, the specified motion must be consistent with a restricted subset of configuration space. This is the constrained motion manifold. Configuration space control presents a fundamental problem since the entire configuration space is assumed accessible and a particular set of arbitrarily chosen configuration space coordinates will likely violate the system constraints. A task space control scheme [17, 18], avoids this problem since, for redundant systems, a point in task space maps to a self-motion manifold [6] in configuration space. As long as the constrained motion and self-motion manifolds intersect, valid constraint consistent and task consistent solutions exist. Task space approaches were employed by Khatib et al. for the control of coordinated multi-arm movements involving loop closure constraints [7, 19, 27].

In addition to controlling motion, it may be desirable to control constraint forces in constrained systems. This can be achieved concurrent to the execution of motion control, provided there is sufficient actuation in the system. Previous work has addressed this but, again, from a configuration space perspective [1, 2, 25]. We will present a natural approach to formulating control for both motion objectives and constraint forces in holonomically constrained systems from a task space perspective. While a task space approach was previously proposed to control motion in holonomically constrained systems [10], this did not address a formal method for simultaneously controlling constraint forces. The approach presented here extends the work of [10]. We begin with a mass-weighted orthogonal decomposition of task, constraint, and posture space, formulated by De Sapio and Park [9]. This decomposition exposes both motion coordinates and constraint forces. Using this decomposition, and the constraint handling methodology presented here, desired constraint forces can be specified and accounted for, along with the motion objectives, in the overall compensation.

Table 1 provides a brief summary of the proposed approach for constrained motion/force control compared to some other approaches. The approach presented here incorporates the unique benefit of being formulated in task space and addressing simultaneous control of motion and constraint force. It does not address redundant constraints as will be discussed in the subsequent sections, however, this is not seen as a major limitation since redundant constraints can be eliminated to form a minimal non-redundant set of constraints.

Table 1 Comparison of some approaches for constrained motion/force control. Configuration (or joint) space control schemes lack the flexibility of task-level specification of motion and force commands
Full size table

We begin in the next section with a standard expression of the unconstrained configuration space dynamics of a multibody system followed by a standard expression of the holonomically constrained configuration space dynamics. We continue with a brief review of the reformulation of these configuration space descriptions into task space descriptions. We present a new methodology for partitioning the task space constraint forces based on particular control objectives. A number of examples are presented to illustrate different control objectives in the combined control of motion and constraint forces. Finally, an approach for solving the constrained control problem given implicit conditions on the constraint forces is detailed. Particular limitations in the approaches presented are also discussed.

2 Configuration space dynamics

The equations of motion for a multibody system that is unconstrained with respect to configuration space can be expressed as [5],

$$ \mathbf {M}(\mathbf {q})\,\ddot{\mathbf {q}} + \mathbf {b}(\mathbf {q},\dot{\mathbf {q}}) + \mathbf {f}(\mathbf {q},\dot{\mathbf {q}}) = \mathbf {B}(\mathbf {q},\dot{\mathbf {q}})^T\mathbf {u}, $$
(1)

where \(\mathbf {q} \in \mathbb {R}^{n}\) is the vector of generalized coordinates, \(\mathbf {u} \in \mathbb {R}^{k}\) is the vector of control inputs, \(\mathbf {B}(\mathbf {q},\dot{\mathbf {q}})^{T} \in \mathbb {R}^{{n} \times {k}}\) is the matrix mapping control inputs to generalized actuator forces, \(\mathbf {M}(\mathbf {q}) \in \mathbb {R}^{{n} \times {n}}\) is the configuration space mass matrix, \(\mathbf {b}(\mathbf {q},\dot{\mathbf {q}}) \in \mathbb {R}^{n}\) is the vector of centrifugal and Coriolis terms, and \(\mathbf {f}(\mathbf {q},\dot{\mathbf {q}}) \in \mathbb {R}^{n}\) is the vector of generalized applied forces. We will often use a modified and more specialized form of (1) common in robotics,

$$ \mathbf {M}(\mathbf {q})\,\ddot{\mathbf {q}} + \mathbf {b}(\mathbf {q},\dot{\mathbf {q}}) + \mathbf {g}(\mathbf {q}) = \mathbf {\tau}, $$
(2)

where \(\mathbf {g}(\mathbf {q}) \in \mathbb {R}^{n}\) is the vector of gravity terms. The form of (2) assumes that the generalized actuator forces, \(\boldsymbol{\tau} \in \mathbb {R}^{n}\), can be directly interpreted as control inputs; that is, τ=B T u=u, i.e., B T=1. Additionally, the generalized applied forces are assumed to be restricted to gravity terms; that is, \(\mathbf {f}(\mathbf {q},\dot{\mathbf {q}}) = \mathbf {g}(\mathbf {q})\).

2.1 Introduction of holonomic constraints

We now introduce a set of m C independent holonomic and scleronomic constraint equations, \(\mathbf {\phi}(\mathbf {q}) = \mathbf {0} \in \mathbb {R}^{{m_{C}}}\), that are satisfied on a p=n−m C dimensional manifold, Q p, in configuration space, \(Q = \mathbb {R}^{n}\). The gradient of ϕ yields the constraint matrix,

$$ \boldsymbol{\Phi}(\mathbf {q}) = \frac{\partial\boldsymbol{\phi}}{\partial \mathbf {q}} \in \mathbb {R}^{{{m_C}} \times {n}}. $$
(3)

Given the stipulation that ϕ(q)=0 are independent relations it follows that Φ is full rank (rank of m C ); that is, the rows of Φ are linearly independent. In fact, if redundancies existed among the constraints this would affect the dimensionality of the constrained motion manifold, Q p, and the rank of the constraint matrix, Φ. Typically, redundant constraints are removed from the model of a multibody system [28]. Redundant constraints will not be addressed in this paper and all subsequent formulations presented here will assume an independent set of constraints (and, thus, full rank constraint matrix). Adjoining the constraints to (2) by introducing a set of constraint forces yields the dynamic equation in the familiar Lagrange multiplier form,

$$ \mathbf {M}\ddot{\mathbf {q}} + \mathbf {b} + \mathbf {g} - \boldsymbol{\Phi}^T \boldsymbol{\lambda} = \boldsymbol{\tau}, $$
(4)

subject to

$$ \boldsymbol{\phi}(\mathbf {q}) = \mathbf {0} \quad \Rightarrow\quad \dot{\boldsymbol{\phi}} = \mathbf {0},\ddot{\boldsymbol{\phi}} = \mathbf {0} \quad \Rightarrow\quad \boldsymbol{\Phi}\dot{\mathbf {q}} = \mathbf {0}, \boldsymbol{ \Phi}\ddot{\mathbf {q}} + \dot{\boldsymbol{\Phi}}\dot{\mathbf {q}} = \mathbf {0}. $$
(5)

3 Task space dynamics and control

In the previous section, we reviewed configuration space descriptions of the dynamics of constrained multibody systems. Our objective is to reformulate these descriptions in task space. This will provide the foundation for constrained task-level control to be discussed in the next section. A brief summary of task space dynamics and control is presented here. For a more detailed exposition of the operational, or task, space approach the reader is referred to [17, 18].

Given a branching chain system and defining a set of m independent task, or operational space, coordinates, \(\mathbf {x} \in \mathbb {R}^{{m}}\) we define the task Jacobian as

$$ \mathbf {J}(\mathbf {q}) = \frac{{\partial \mathbf {x}}}{{\partial{ \mathbf {q}}}} \in \mathbb {R}^{{m} \times {n}}. $$
(6)

These task space coordinates are related to the generalized coordinates by a functional mapping, and are chosen as a more natural space from which to formulate the control problem. As an example, they can be chosen as the Cartesian coordinates of some effector frame of the multibody system. As with the constraints, given the stipulation that x are independent coordinates it follows that J is full rank (rank of m); that is, the rows of J are linearly independent. Redundant task coordinates will not be addressed in this paper and all subsequent formulations presented here will assume an independent set of task coordinates (and, thus, full rank task Jacobian matrix).

The generalized actuator force (or control torque) in (2) can then be composed as J T f, where \(\mathbf {f} \in \mathbb {R}^{m}\) is the task, or operational space, force. In the redundant case, an additional term needs to complement the task term in order to realize any arbitrary generalized force. We will refer to this term as the null space term, and it can be composed as N T τ ∗, where τ ∗ is an arbitrary generalized force and \(\mathbf {N}(\mathbf {q})^{T} \in \mathbb {R}^{{n} \times {n}}\) is the null space projection matrix. We then have the following set of unconstrained task, or operational space, equations of motion [17],

$$ \boldsymbol{\Lambda}(\mathbf {q})\,\ddot{\mathbf {x}} + \boldsymbol{\mu}( \mathbf {q},\dot{\mathbf {q}}) + \mathbf {p}(\mathbf {q}) = \mathbf {f}, $$
(7)

where \({\boldsymbol{\Lambda}}(\mathbf {q}) \in \mathbb {R}^{{m} \times {m}}\) is the operational space mass matrix, \({\boldsymbol{\mu}}(\mathbf {q},\dot{\mathbf {q}}) \in \mathbb {R}^{m}\) is the operational space centrifugal and Coriolis force vector, and \({\mathbf {p}}(\mathbf {q}) \in \mathbb {R}^{m}\) is the operational space gravity vector. These terms are given by

$$\begin{aligned} \boldsymbol{\Lambda}(\mathbf {q}) & = \bigl(\mathbf {JM}^{-1} \mathbf {J}^T\bigr)^{-1}, \end{aligned}$$
(8)
$$\begin{aligned} \boldsymbol{\mu}(\mathbf {q},\dot{\mathbf {q}}) & = \boldsymbol{\Lambda} \mathbf {J} \mathbf {M}^{-1} \mathbf {b} - \boldsymbol{\Lambda}\dot{\mathbf {J}}\dot{\mathbf {q}}, \end{aligned}$$
(9)
$$\begin{aligned} \mathbf {p}(\mathbf {q}) & = \boldsymbol{\Lambda} \mathbf {J}\mathbf {M}^{-1} \mathbf {g}, \end{aligned}$$
(10)
$$\begin{aligned} \mathbf {N}(\mathbf {q})^T & = \mathbf {1} - \mathbf {J}^T \boldsymbol{\Lambda} \mathbf {J}\mathbf {M}^{-1}. \end{aligned}$$
(11)

Thus, the overall dynamics of our multibody system, given by

$$ \mathbf {M} \ddot{\mathbf {q}} + \mathbf {b} + \mathbf {g} = \mathbf {J}^T \mathbf {f} + \mathbf {N}^T \boldsymbol{\tau}_* = \boldsymbol{\tau}, $$
(12)

can be mapped into task space,

$$ {\mathbf {M} \ddot{\mathbf {q}}} + {\mathbf {b}} + {\mathbf {g}} = {\boldsymbol{\tau}}\, \mathop{\to}\limits ^{\bar{\mathbf {J}}^T} \, {\mathbf {f}} = {\boldsymbol{ \Lambda}\ddot{\mathbf {x}}} + {\boldsymbol{\mu}} + {\mathbf {p}}, $$
(13)

where \(\bar{\mathbf {J}}\) is the dynamically consistent inverse of the task Jacobian,

$$ \bar{\mathbf {J}} = \mathbf {M}^{-1} \mathbf {J}^T \boldsymbol{ \Lambda}. $$
(14)

In a complementary manner, the overall dynamics can be mapped into the task consistent null space (or posture space) using N T.

The overall dynamics can be expressed as

$$ \mathbf {J}^T (\boldsymbol{\Lambda}\ddot{\mathbf {x}} + \boldsymbol{\mu} + \mathbf {p}) + \mathbf {N}^T\boldsymbol{\tau}_* = \boldsymbol{\tau}. $$
(15)

A controller employing (7) would be assumed to have imperfect knowledge of the system. Therefore, (7) should reflect estimates for the inertial and gravitational terms. Additionally, a control law needs to be incorporated. To this end, we replace \(\ddot{\mathbf {x}}\) in (7) with the input of the decoupled system [18], f ⋆, to yield the dynamic compensation equation,

$$ \mathbf {f} = \widehat{\boldsymbol{\Lambda}} \mathbf {f}^\star+ \widehat{\boldsymbol{\mu}} + \widehat{\mathbf {p}}, $$
(16)

where the \(\text{ }\widehat{\cdot}\text{ }\) represents estimates of the dynamic properties. Thus our control torque is,

$$ \boldsymbol{\tau} = \mathbf {J}^T \bigl(\widehat{\boldsymbol{\Lambda}} \mathbf {f}^\star+ \widehat{\boldsymbol{\mu}} + \widehat{\mathbf {p}}\bigr) + \widehat{\mathbf {N}}^T\boldsymbol{\tau}_*. $$
(17)

Any suitable control law can be chosen to serve as input of the decoupled system. In particular, we can choose a linear proportional-derivative (PD) control law of the form

$$ \mathbf {f}^\star = \mathbf {K}_{x}(\mathbf {x}_{d} - \mathbf {x}) + \mathbf {K}_{v}(\dot{\mathbf {x}}_{d} - \dot{\mathbf {x}}) + \ddot{\mathbf {x}}_{d}, $$
(18)

where x d are reference values for the task coordinates and K x and K v are gain matrices.

3.1 Introduction of holonomic constraints

In this section, we will review task space methodologies for addressing constrained systems. As we introduced a set of m C holonomic and scleronomic constraint equations to the configuration space dynamics, we can do the same for the task space dynamics. Mapping (4) and (5) into an appropriate task/constraint space yields

$$ \boldsymbol{\Lambda}\ddot{\mathbf {x}} + \boldsymbol{\mu} + \mathbf {p} -\bar{\mathbf {J}}^T\boldsymbol{\Phi}^T(\boldsymbol{\alpha} + \boldsymbol{\rho}) = \bar{\mathbf {J}}^T\boldsymbol{\Theta}^T\boldsymbol{\tau}. $$
(19)

This expression is derived in detail in [10]. The term \(\boldsymbol{\alpha}(\mathbf {q}, \dot{\mathbf {q}}) \in \mathbb {R}^{m_{C}}\) is the vector of centrifugal and Coriolis forces projected at the constraint, and \(\boldsymbol{\rho}(\mathbf {q}) \in \mathbb {R}^{m_{C}}\) is the vector of gravity forces projected at the constraint. These terms are given by

$$\begin{aligned} \boldsymbol{\alpha}(\mathbf {q}, \dot{\mathbf {q}}) & = \mathbf {H}\boldsymbol{\Phi } \mathbf {M}^{-1}\mathbf {b} - \mathbf {H}\dot{\boldsymbol{\Phi}}\dot{\mathbf {q}}, \end{aligned}$$
(20)
$$\begin{aligned} \boldsymbol{\rho}(\mathbf {q}) & = \mathbf {H}\boldsymbol{\Phi} \mathbf {M}^{-1} \mathbf {g}, \end{aligned}$$
(21)

where \(\mathbf {H}(\mathbf {q}) \in \mathbb {R}^{{m_{C}} \times {m_{C}}}\) is the constraint space mass matrix which reflects the system inertia projected at the constraint

$$ \mathbf {H}(\mathbf {q}) = \bigl(\boldsymbol{\Phi} \mathbf {M}^{-1}\boldsymbol{ \Phi}^T\bigr)^{-1}. $$
(22)

The constraint null space projection matrix, \(\boldsymbol{\Theta}(\mathbf {q})^{T} \in \mathbb {R}^{{n} \times {n}}\), is given by

$$ \boldsymbol{\Theta}(\mathbf {q})^T = \mathbf {1} - \boldsymbol{ \Phi}^T\bar{\boldsymbol{\Phi}}^T, $$
(23)

where \(\bar{\boldsymbol{\Phi}}\) is the dynamically consistent inverse of Φ,

$$ \bar{\boldsymbol{\Phi}} = \mathbf {M}^{-1}\boldsymbol{\Phi}^T \mathbf {H}. $$
(24)

The control equation can be expressed as

$$ \bar{\mathbf {J}}^T\widehat{\boldsymbol{\Theta}}^T\boldsymbol{\tau} = \widehat{\boldsymbol{\Lambda}} \mathbf {f}^\star+ \widehat{\boldsymbol{\mu}} + \widehat{\mathbf {p}} - \bar{\mathbf {J}}^T\boldsymbol{\Phi}^T( \widehat{\boldsymbol{\alpha}} + \widehat{\boldsymbol{\rho}}), $$
(25)

where the linear control law of (18) can be used.

It is noted that (19) does not expose the constraint forces (Lagrange multipliers). An alternate form of the constrained task space dynamics is [8, 9]

$$ \boldsymbol{\Theta}^T\mathbf {J}^T(\boldsymbol{ \Lambda}_c\ddot{\mathbf {x}} + \boldsymbol{\mu}_c + \mathbf {p}_c) + \boldsymbol{\Phi}^T(\boldsymbol{\alpha} + \boldsymbol{\rho}) + \mathbf {N}_c^T\boldsymbol{\tau}_* - \boldsymbol{ \Phi}^T\boldsymbol{\lambda} = \boldsymbol{\tau}. $$
(26)

This is derived from the task-level orthogonalization procedure [9] summarized in Algorithm 1 for a single task (n T =1).

Algorithm 1
figure 1

Task-level orthogonalization with constraints

Full size image

The term \({\boldsymbol{\Lambda}_{c}}(\mathbf {q}) \in \mathbb {R}^{{m} \times {m}}\) is the task/constraint space mass matrix, \({\boldsymbol{\mu}_{c}}(\mathbf {q},\dot{\mathbf {q}}) \in \mathbb {R}^{m}\) is the task/constraint space centrifugal and Coriolis force vector, \({\mathbf {p}_{c}}(\mathbf {q}) \in \mathbb {R}^{m}\) is the task/constraint space gravity vector, and \({\mathbf {N}_{c}}(\mathbf {q})^{T} \in \mathbb {R}^{{n} \times {n}}\) is the task/constraint null space projection matrix. These terms are given by

$$\begin{aligned} \boldsymbol{\Lambda}_c(\mathbf {q}) & = \bigl(\mathbf {J} \mathbf {M}^{- 1}\boldsymbol{\Theta }^T \mathbf {J}^T \bigr)^{- 1}, \end{aligned}$$
(27)
$$\begin{aligned} \boldsymbol{\mu}_c(\mathbf {q},\dot{\mathbf {q}}) & = \boldsymbol{ \Lambda}_c\mathbf {J}\mathbf {M}^{- 1}\boldsymbol{ \Theta}^T\mathbf {b} - \boldsymbol{\Lambda}_c\bigl(\dot{\mathbf {J}} - \mathbf {J}\mathbf {M}^{- 1}\boldsymbol{\Phi}^T \mathbf {H}\dot{\boldsymbol{\Phi}}\bigr)\dot{\mathbf {q}}, \end{aligned}$$
(28)
$$\begin{aligned} \mathbf {p}_c(\mathbf {q}) & = \boldsymbol{\Lambda}_c \mathbf {J}\mathbf {M}^{- 1}\boldsymbol{\Theta}^T\mathbf {g}, \end{aligned}$$
(29)
$$\begin{aligned} \mathbf {N}_c(\mathbf {q})^T &= \boldsymbol{ \Theta}^T\bigl(\mathbf {1} - \mathbf {J}^T\boldsymbol{ \Lambda}_c\mathbf {J}\boldsymbol{\Theta} \mathbf {M}^{- 1}\bigr). \end{aligned}$$
(30)

Equation (26) expresses the control torque as a function of the task accelerations, \(\ddot{\mathbf {x}}\), the kinematic and dynamic properties, and the constraint forces, λ. Employing a linear control law the control equation can be expressed as

$$ \boldsymbol{\tau} + \boldsymbol{\Phi}^T\boldsymbol{\lambda} = \widehat{\boldsymbol{\Theta}}^T\mathbf {J}^T\bigl( \widehat{\boldsymbol{\Lambda}}_c\mathbf {f}^\star+ \widehat{\boldsymbol{\mu}}_c + \widehat{\mathbf {p}}_c\bigr) + \boldsymbol{ \Phi}^T(\widehat{\boldsymbol{\alpha}} + \widehat{\boldsymbol{\rho}}) + \widehat{\mathbf {N}}_c^T\boldsymbol{\tau}_*, $$
(31)

where

$$ \mathbf {f}^\star = \mathbf {K}_x(\mathbf {x}_d - \mathbf {x}) + \mathbf {K}_v(\dot{\mathbf {x}}_d - \dot{\mathbf {x}}) + \ddot{\mathbf {x}}_d. $$
(32)

These equations need to be complemented by the condition on the unactuated joints,

$$ \mathbf {S}_p\, \boldsymbol{\tau} = \mathbf {0}, $$
(33)

where given k actuated joints, \(\mathbf {S}_{p} \in \mathbb {R}^{{(n-k)} \times {n}}\) is a selection matrix that identifies the passive (unactuated) joints.

3.2 Partitioning the constraint forces

We note that the rank of \(\mathbf {N}_{c}^{T} \in \mathbb {R}^{{n} \times {n}}\) is N, the dimension of the null space. Therefore, we can generate an N-dimensional basis for \(\text {im}(\mathbf {N}_{c}^{T})\). We will construct the columns of \(\mathbf {U}_{c}^{T} \in \mathbb {R}^{{n} \times {N}}\) from this basis such that \(\text {im}(\mathbf {U}_{c}^{T}) = \text {im}(\mathbf {N}_{c}^{T})\). We can then express the null space torque as \(\mathbf {U}_{c}^{T}\boldsymbol{\tau}_{N}\) where \(\boldsymbol{\tau}_{N} \in \mathbb {R}^{N}\) is the control vector for the null space. Thus we can express (31) as

$$ \boldsymbol{\tau} = \widehat{\boldsymbol{\Theta}}^T\mathbf {J}^T \bigl(\widehat{\boldsymbol{\Lambda}}_c\mathbf {f}^\star+ \widehat{\boldsymbol{\mu}}_c + \widehat{\mathbf {p}}_c\bigr) + \boldsymbol{\Phi}^T(\widehat{\boldsymbol{\alpha}} + \widehat{\boldsymbol{\rho}}) - \boldsymbol{\Phi}^T\boldsymbol{\lambda} + \widehat{\mathbf {U}}_c^T\boldsymbol{\tau}_N, $$
(34)

complemented by

$$ \mathbf {S}_p\, \boldsymbol{\tau} = \mathbf {0}. $$
(35)

It is noted that this system is well conditioned only if our previously stated stipulations that independent constraints and task coordinates are specified. Furthermore, the task conditions must be independent of the constraints. That is, a task cannot be specified to produce motion that would violate the constraints. Additionally, given that estimates are used for the inertial parameters it is conceivable that choices for these estimates could make the system ill-conditioned or singular, in which case exact solutions for τ would not exist.

We will now review the dimensionality of our control problem. Given a system with n generalized coordinates and m C holonomic constraints, there are p=n−m C degrees of freedom characterizing the constrained motion. This motion space can be controlled by specifying a task with m task coordinates. The null space dimensionality, N=p−m, characterizes the remaining dimensionality of the constrained motion space not used by the task. This is illustrated in Fig. 1. Given k actuated joints, we will refer to the condition that k≥p as motion actuated and the condition that k≥m as task actuated [10].

Fig. 1
figure 2

The relationship between the number of generalized coordinates, n, degrees of freedom, p, constraints, m C , task coordinates, m, and null space coordinates, N. The system is motion actuated if k≥p and task actuated if k≥m, where k is the number of actuators

Full size image

The control equations given by (34) and (35) represent a system of 2n−k scalar equations, where \(\boldsymbol{\tau} \in \mathbb {R}^{n}\) constitute n unknowns to be solved for. The system is overdetermined so we need additional n−k unknowns. The right-hand side (RHS) of (34) contains the variables \(\mathbf {f}^{\star}\in \mathbb {R}^{m}\), \(\boldsymbol{\tau}_{N} \in \mathbb {R}^{N}\), and \(\boldsymbol{\lambda} \in \mathbb {R}^{m_{C}}\). The total dimensionality of these variables is m+N+m C =p+m C =n. These n RHS variables can be partitioned into k variables which can be specified as part of the control and n−k variables which can serve as unknowns to be moved to the left hand side (LHS).

3.2.1 Motion actuated partitioning

We may assume that we wish to specify the task coordinates and the null space coordinates as part of the control. This represents m task coordinates and N null space coordinates for a total of m+N=p motion coordinates to be specified as part of the control. If the system is motion actuated (k≥p) then there is sufficient actuation to control these coordinates. Further, if k>p, then k−p constraint forces can also be controlled in addition to the p motion coordinates. This is illustrated in Fig. 2.

Fig. 2
figure 3

If k>p, then the n RHS variables of (34) can be partitioned into k controlled variables: p controlled motion coordinates and k−p controlled constraint forces; and n−k uncontrolled constraint forces

Full size image

It has been shown that multibody systems with redundant constraints do not have a unique solution for joint reaction forces [28, 29]. This is not an issue since the approach outlined here is limited to the assumption that no redundancies exist among the constraints (or, if they do, that they are eliminated from the constraint model prior to utilizing the formulation presented here).

In this case, let us introduce a selection matrix, \(\mathbf {S}_{c} \in \mathbb {R}^{{(k - p)} \times {m_{C}}}\), to select the controlled constraint forces and a selection matrix, \(\mathbf {S}_{u} \in \mathbb {R}^{{(n - k)} \times {m_{C}}}\), to select the uncontrolled constraint forces. That is,

$$ \boldsymbol{\lambda}_c = \mathbf {S}_c\, \boldsymbol{\lambda} \quad \text {and} \quad \boldsymbol{\lambda}_u = \mathbf {S}_u\, \boldsymbol{\lambda}, $$
(36)

or

$$ \left ( \begin{array}{*{20}c} {\boldsymbol{\lambda}_c} \\ {\boldsymbol{\lambda}_u} \\ \end{array} \right ) = \left ( \begin{array}{*{20}c} {\mathbf {S}_c} \\ {\mathbf {S}_u} \\ \end{array} \right ) \boldsymbol{\lambda}, $$
(37)

where λ c and λ u are the vectors of controlled and uncontrolled constraint forces, respectively, selected out of the full vector of constraint forces, λ. Inverting (37), we have

$$ \boldsymbol{\lambda} = \left ( \begin{array}{c@{\quad}c} {\mathbf {S}_c^T} & {\mathbf {S}_u^T} \\ \end{array} \right ) \left ( \begin{array}{c} {\boldsymbol{\lambda}_c} \\ {\boldsymbol{\lambda}_u} \\ \end{array} \right ). $$
(38)

Substituting this into (34), we have

$$ \boldsymbol{\tau} + \boldsymbol{\Phi}^T \mathbf {S}_u^T \boldsymbol{\lambda}_u = \widehat{\boldsymbol{\Theta}}^T \mathbf {J}^T\bigl(\widehat{\boldsymbol{\Lambda}}_c\mathbf {f}^\star+ \widehat{\boldsymbol{\mu}}_c + \widehat{\mathbf {p}}_c\bigr) + \boldsymbol{\Phi}^T\bigl(\widehat{\boldsymbol{\alpha}} + \widehat{\boldsymbol{\rho}} - \mathbf {S}_c^T\boldsymbol{ \lambda}_{c_d}\bigr) + \widehat{\mathbf {U}}_c^T \boldsymbol{\tau}_N, $$
(39)

where λ c has been replaced with \(\boldsymbol{\lambda }_{c_{d}}\) to denote that it is specified as part of the control reference, along with x d , \(\dot{\mathbf {x}}_{d}\), \(\ddot{\mathbf {x}}_{d}\), and τ N . We will define

$$ \mathbf {h}(\mathbf {q},\dot{\mathbf {q}}) \triangleq\widehat{\boldsymbol{\Theta}}^T\mathbf {J}^T\bigl(\widehat{\boldsymbol{\Lambda}}_c \mathbf {f}^\star+ \widehat{\boldsymbol{\mu}}_c + \widehat{\mathbf {p}}_c\bigr) + \boldsymbol{\Phi}^T \bigl(\widehat{\boldsymbol{\alpha}} + \widehat{\boldsymbol{\rho}} - \mathbf {S}_c^T \boldsymbol{\lambda}_{c_d}\bigr) + \widehat{\mathbf {U}}_c^T \boldsymbol{\tau}_N. $$
(40)

Thus, (39) and (35) can be represented as the following system of 2n−k equations:

$$ \left ( { \begin{array}{c@{\quad}c} \mathbf {1} & {\boldsymbol{\Phi}^T \mathbf {S}_u^T} \\ {\mathbf {S}_p} & {\mathbf {0}} \\ \end{array} } \right )\left ( { \begin{array}{c} {\boldsymbol{\tau}} \\ {\boldsymbol{\lambda}_u} \\ \end{array} } \right ) = \left ( { \begin{array}{c} {\mathbf {h}(\mathbf {q},\dot{\mathbf {q}})} \\ {\mathbf {0}} \\ \end{array} } \right ). $$
(41)

Given the inverse

$$ \left ( { \begin{array}{c@{\quad}c} \mathbf {1} & {\boldsymbol{\Phi}^T \mathbf {S}_u^T} \\ {\mathbf {S}_p} & {\mathbf {0}} \\ \end{array} } \right )^{-1} = \left ( { \begin{array}{c@{\quad}c} {\mathbf {1} - \boldsymbol{\Phi}^T \mathbf {S}_u^T(\mathbf {S}_p\boldsymbol{\Phi}^T \mathbf {S}_u^T)^{ - 1} \mathbf {S}_p\;\;} & {\boldsymbol{\Phi}^T \mathbf {S}_u^T(\mathbf {S}_p\boldsymbol{\Phi}^T \mathbf {S}_u^T)^{ - 1}} \\ {(\mathbf {S}_p\boldsymbol{\Phi}^T \mathbf {S}_u^T)^{ - 1} \mathbf {S}_p} & { - (\mathbf {S}_p\boldsymbol{\Phi}^T \mathbf {S}_u^T)^{ - 1} } \\ \end{array} } \right ), $$
(42)

we have the following solution for the control torque:

$$ \boldsymbol{\tau} = \bigl(\mathbf {1} - \boldsymbol{\Phi}^T \mathbf {S}_u^T\bigl(\mathbf {S}_p\boldsymbol{ \Phi}^T \mathbf {S}_u^T\bigr)^{ - 1} \mathbf {S}_p\bigr)\mathbf {h}(\mathbf {q},\dot{\mathbf {q}}). $$
(43)

A block diagram of this control scheme is shown in Fig. 3.

Fig. 3
figure 4

A task space tracking controller for a constrained plant. The desired task motion and constraint forces are tracked using appropriate dynamic compensation which accounts for the constraints

Full size image

3.2.2 Example 1

As an illustrative example of this control scheme, we consider the parallel mechanism depicted in Fig. 4(left), where n=9, m C =6, and p=3. Units for this and all subsequent examples will be expressed in SI units (meters, seconds, and Newtons). The constraint equations describe the loop closures and are given by [8]

$$ \boldsymbol{\phi} (\mathbf {q}) = \left ( { \begin{array}{c} \mathbf {r}_{p_1} - \mathbf {r}_{l_1} \\ \mathbf {r}_{p_2} - \mathbf {r}_{l_2} \\ \mathbf {r}_{p_3} - \mathbf {r}_{l_3} \\ \end{array} } \right ). $$
(44)

Considering two of the base joints, q 1, q 3, as well as the elbow joints, q 2, q 4, and q 6, to be actuated, we have k=5 and

$$ \mathbf {S}_p = \left ( { \begin{array}{c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c} 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \\ \end{array} } \right ). $$
(45)
Fig. 4
figure 5

(Left) Parallel mechanism consisting of serial chains with loop closures. Two base joints, q 1, q 3, as well as the elbow joints, q 2, q 4, and q 6, are actuated while the remaining joints are passive. (Right) The position of the platform is commanded to move to a target while its orientation is uncontrolled. In this case, n=9, m C =6, p=3, m=2, N=1, and k=5

Full size image

We will define the task to control the position of the platform (see Fig. 4(right)) while its orientation is uncontrolled. That is, m=2, N=1, and

$$ \mathbf {x} \triangleq( { \begin{array}{c@{\quad}c} q_7 & q_8 \\ \end{array} } )^T. $$
(46)

We will specify the reference value as

$$ \mathbf {x}_d = ( { \begin{array}{c@{\quad}c} -0.25 & 2.75 \\ \end{array} } )^T. $$
(47)

The null space torque will be specified to be zero (τ N =0). Additionally, we wish to specify the constraint forces at the interface of \(\mathbf {r}_{p_{1}}\) and \(\mathbf {r}_{l_{1}}\). These correspond to

$$ \boldsymbol{\lambda}_c \triangleq \left ( { \begin{array}{c@{\quad}c} \lambda_1 & \lambda_2 \\ \end{array} } \right )^T. $$
(48)

We will specify the reference value as

$$ \boldsymbol{\lambda}_{c_d} = \left ( { \begin{array}{c@{\quad}c} 25\sin(t/50) & 150\cos(t/200) \\ \end{array} } \right )^T. $$
(49)

The remaining constraint forces will be unspecified. Thus,

$$ \mathbf {S}_c = \left ( { \begin{array}{c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ \end{array} } \right ) $$
(50)

and

$$ \mathbf {S}_u = \left ( { \begin{array}{c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c} 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \\ \end{array} } \right ). $$
(51)

The linear (PD) control law of (32) is used as the input of the decoupled system. The gains are chosen so as to achieve critically damped behavior of the task motion. Equation (43) is used to compute the control torque. The system dynamics were computed symbolically using a Lagrangian formulation and solved numerically. Figure 5 shows simulation plots for the system under goal position commands on the task coordinates, x, and sinusoidal tracking commands on the constraint force coordinates, λ c . The time response of the platform position shows linear critically damped motion to the target but the time response of the platform orientation shows undamped oscillation due to the uncontrolled null space.

Fig. 5
figure 6

The position of the platform is commanded to move to a target while its orientation is uncontrolled and the constraint forces at the interface of \(\mathbf {r}_{p_{1}}\) and \(\mathbf {r}_{l_{1}}\) are specified. (Left) Time response of the platform position showing linear critically damped motion to the target. Time response of the platform orientation shows undamped null space oscillation due to zero null space torque. The control gains are K x =100 and K v =20. (Right) Time response of the controlled constraint forces, λ 1 and λ 2, showing tracking of the reference command

Full size image

In this and all subsequent simulations in this paper, the constrained dynamics were numerically integrated with a time step of 1 ms. Baumgarte stabilization [4] was used to stabilize the constraints. Given the constraint stabilized system of equations [11],

$$ \left ( { \begin{array}{c@{\quad}c} \mathbf {M} & - \boldsymbol{\Phi}^T \\ - \boldsymbol{\Phi} & \mathbf {0} \\ \end{array} } \right )\left ( { \begin{array}{c} \ddot{\mathbf {q}} \\ \boldsymbol{\lambda} \\ \end{array} } \right ) = \left ( { \begin{array}{c} \boldsymbol{\tau} - \mathbf {b} - \mathbf {g} \\ \dot{\boldsymbol{\Phi}}\dot{\mathbf {q}} + \beta\boldsymbol{\Phi}\dot{\mathbf {q}} + \alpha\boldsymbol{\phi} \\ \end{array} } \right ), $$
(52)

where α and β are the constraint stabilization parameters, the generalized accelerations are

$$ \ddot{\mathbf {q}} = \boldsymbol{\Theta} \mathbf {M}^{-1}(\boldsymbol{\tau} - \mathbf {b} - \mathbf {g}) - \bar{\boldsymbol{\Phi}}\dot{\boldsymbol{\Phi}}\dot{\mathbf {q}} - \bar{\boldsymbol{\Phi}}(\alpha\boldsymbol{\phi} + \beta\dot{\boldsymbol{\phi}}), $$
(53)

and the Lagrange multipliers are

$$ \boldsymbol{\lambda} = -\bar{\boldsymbol{\Phi}}^T(\boldsymbol{\tau} - \mathbf {b} - \mathbf {g}) - \mathbf {\mathrm{H}}\dot{\boldsymbol{\Phi}}\dot{\mathbf {q}} - \mathbf { \mathrm{H}}(\alpha \boldsymbol{\phi} + \beta\dot{\boldsymbol{\phi}}). $$
(54)

Figure 6 shows simulation plots of some of the control torques generated for this motion. It is noted that zero control torque is produced at the passive joint, τ 5, due to the condition of (35). The last plot shows the time response of one of the uncontrolled constraint forces.

Fig. 6
figure 7

(Top) Time response of the control torques τ 1 and τ 2 during goal movement. (Bottom left) Zero control torque (numerical error at the order of 10−12) is produced at the passive joint τ 5, due to the imposition of the passivity requirement in the controller. (Bottom right) Time response of one of the uncontrolled constraint forces, λ 4

Full size image

3.2.3 Example 2

We can choose to explicitly control the platform orientation as part of the task. In this case the task is defined as

$$ \mathbf {x} \triangleq( { \begin{array}{c@{\quad}c@{\quad}c} q_7 & q_8 & q_9\\ \end{array} } )^T. $$
(55)

The null space vanishes with this expansion of the task space. We will specify the reference value as

$$ \mathbf {x}_d = ( { \begin{array}{c@{\quad}c@{\quad}c} -0.25 & 2.75 & -0.5\\ \end{array} } )^T. $$
(56)

We note that n=9, m C =6, p=3, m=3, N=0, and k=5.

The procedure of the previous example is used with this new task definition. Figure 7 shows simulation plots for the system under goal position commands on the task coordinates, x, and sinusoidal tracking commands on the constraint force coordinates, λ c . As with the position coordinates the time response of the platform orientation now shows a linear critically damped response to the target.

Fig. 7
figure 8

The position of the platform is commanded to move to a target while its orientation is also controlled and the constraint forces at the interface of \(\mathbf {r}_{p_{1}}\) and \(\mathbf {r}_{l_{1}}\) are specified. (Left) Time response of the platform position and orientation showing linear critically damped motion to the target. The control gains are K x =100 and K v =20. (Right) Time response of the controlled constraint forces, λ 1 and λ 2, showing tracking of the reference command

Full size image

Figure 8 shows simulation plots of some of the control torques generated for this motion. It is noted that zero control torque is produced at the passive joint, τ 5, due to the condition of (35). The last plot shows the time response of the uncontrolled constraint forces.

Fig. 8
figure 9

(Top) Time response of the control torques τ 1 and τ 2 during goal movement. (Bottom left) Zero control torque (numerical error at the order of 10−12) is produced at the passive joint τ 5, due to the passivity requirement. (Bottom right) Time response of the uncontrolled constraint forces, λ 3, λ 4, λ 5, and λ 6

Full size image

3.2.4 Task actuated partitioning

In the preceding formulation for partitioning the constraint forces we specified the task coordinates and the null space coordinates as part of the control. It is noted that we are not required to do this and can, for example, dispense with controlling the null space coordinates so that we may control more of the constraint forces. In this case, rather than needing to meet the condition that k≥p, we would only need to meet the condition that k>m in order to control some of the constraint forces. In the more general case of controlling some, or possibly none, of the constraint forces we would only need to meet the task-actuated condition of k≥m. This is illustrated in Fig. 9.

Fig. 9
figure 10

If k>m, then the n RHS variables of (34) can be partitioned into k controlled variables: m controlled task coordinates and k−m controlled constraint forces; and n−N−k uncontrolled constraint forces and N uncontrolled null space coordinates

Full size image

In this case, the selection matrix for the controlled constraint forces would be \(\mathbf {S}_{c} \in \mathbb {R}^{{(k - m)} \times {m_{C}}}\) and the selection matrix for the uncontrolled constraint forces would be \(\mathbf {S}_{u} \in \mathbb {R}^{{(n - N - k)} \times {m_{C}}}\). Our system would then be partitioned as

$$ \boldsymbol{\tau} + \boldsymbol{\Phi}^T \mathbf {S}_u^T \boldsymbol{\lambda}_u - \widehat{\mathbf { U}}_c^T \boldsymbol{\tau}_N = \widehat{\boldsymbol{\Theta}}^T \mathbf {J}^T\bigl(\widehat{\boldsymbol{\Lambda}}_c \mathbf {f}^\star+ \widehat{\boldsymbol{\mu}}_c + \widehat{\mathbf {p}}_c\bigr) + \boldsymbol{\Phi}^T\bigl( \widehat{\boldsymbol{\alpha}} + \widehat{\boldsymbol{\rho}} - \mathbf {S}_c^T\boldsymbol{ \lambda}_{c_d}\bigr), $$
(57)

complemented by

$$ \mathbf {S}_p\, \boldsymbol{\tau} = \mathbf {0}. $$
(58)

Defining

$$ \mathbf {k}(\mathbf {q},\dot{\mathbf {q}}) \triangleq\widehat{\boldsymbol{\Theta }}^T\mathbf {J}^T\bigl(\widehat{\boldsymbol{\Lambda}}_c \mathbf {f}^\star+ \widehat{\boldsymbol{\mu}}_c + \widehat{\mathbf {p}}_c\bigr) + \boldsymbol{\Phi}^T\bigl(\widehat{\boldsymbol{\alpha}} + \widehat{\boldsymbol{\rho}} - \mathbf {S}_c^T\boldsymbol{\lambda}_{c_d} \bigr), $$
(59)

we have the following system of 2n−k equations:

$$ \left ( { \begin{array}{c@{\quad}c@{\quad}c} \mathbf {1} & {\boldsymbol{\Phi}^T}\mathbf {S}_u^T & - \widehat{\mathbf {U}}_c^T \\ {\mathbf {S}_p} & {\mathbf {0}} & {\mathbf {0}} \\ \end{array} } \right )\left ( { \begin{array}{c} {\boldsymbol{\tau}} \\ {\boldsymbol{\lambda}_u} \\ {\boldsymbol{\tau}_N} \\ \end{array} } \right ) = \left ( { \begin{array}{c} {\mathbf {k}(\mathbf {q},\dot{\mathbf {q}})} \\ {\mathbf {0}} \\ \end{array} } \right ). $$
(60)

3.2.5 Example 3

As an illustrative example we will address the specific case of k=m. In this case, we can only control the task, and not the null space or any constraint forces (see Fig. 10).

Fig. 10
figure 11

If k=m, then only the task coordinates can be controlled. The m C constraint forces and N null space coordinates are uncontrolled

Full size image

This represents an underactuated system with respect to motion. Since it is still task actuated it can be controlled. In fact, this methodology represents a means of controlling underactuated systems. The system can be represented as

$$ \boldsymbol{\tau} + \boldsymbol{\Phi}^T\boldsymbol{\lambda} - \widehat{\mathbf {U}}_c^T\boldsymbol{\tau}_N= \widehat{\boldsymbol{\Theta}}^T\mathbf {J}^T\bigl( \widehat{\boldsymbol{\Lambda}}_c\mathbf {f}^\star+ \widehat{\boldsymbol{\mu}}_c + \widehat{\mathbf {p}}_c\bigr) + \boldsymbol{ \Phi}^T(\widehat{\boldsymbol{\alpha}} + \widehat{\boldsymbol{\rho}}), $$
(61)

complemented by

$$ \mathbf {S}_p\, \boldsymbol{\tau} = \mathbf {0}. $$
(62)

Defining

$$ \mathbf {s}(\mathbf {q},\dot{\mathbf {q}}) \triangleq\widehat{\boldsymbol{\Theta }}^T\mathbf {J}^T\bigl(\widehat{\boldsymbol{\Lambda}}_c \mathbf {f}^\star+ \widehat{\boldsymbol{\mu}}_c + \widehat{\mathbf {p}}_c\bigr) + \boldsymbol{\Phi}^T(\widehat{\boldsymbol{\alpha}} + \widehat{\boldsymbol{\rho}}), $$
(63)

we have the following system of 2n−m equations:

$$ \left ( { \begin{array}{c@{\quad}c@{\quad}c} \mathbf {1} & {\boldsymbol{\Phi}^T} & - \widehat{\mathbf {U}}_c^T \\ {\mathbf {S}_p} & {\mathbf {0}} & {\mathbf {0}} \\ \end{array} } \right )\left ( { \begin{array}{c} {\boldsymbol{\tau}} \\ {\boldsymbol{\lambda}} \\ {\boldsymbol{\tau}_N} \\ \end{array} } \right ) = \left ( { \begin{array}{c} {\mathbf {s}(\mathbf {q},\dot{\mathbf {q}})} \\ {\mathbf {0}} \\ \end{array} } \right ). $$
(64)

Considering only the elbow joints q 4, and q 6, to be actuated we have

$$ \mathbf {S}_p = \left ( { \begin{array}{c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c} 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \\ \end{array} } \right ). $$
(65)

We define the task to control the position of the platform

$$ \mathbf {x} \triangleq( { \begin{array}{c@{\quad}c} q_7 & q_8 \\ \end{array} } )^T. $$
(66)

Since only two joints are actuated we can not control the null space or any of the constraint forces. We specify the task reference value as

$$ \mathbf {x}_d = ( { \begin{array}{c@{\quad}c} -0.25 & 2.75 \\ \end{array} } )^T. $$
(67)

We note that n=9, m C =6, p=3, m=2, N=1, and k=2.

The linear (PD) control law of (32) is used as the input of the decoupled system. The gains are chosen so as to achieve critically damped behavior of the task motion. Equation (64) is solved to compute the control torque. Figure 11 shows simulation plots for the system under goal position commands on the task coordinates, x. The time response of the platform position shows linear critically damped motion to the target but the time response of the platform orientation shows undamped oscillation due to the null space torques arising to satisfy the passivity requirement. The control torques generated for this motion are shown. It is noted that zero control torque is produced at the passive joints, τ 1, τ 2, τ 3, and τ 5, due to the passivity condition. The last plot shows the time response of the uncontrolled constraint forces.

Fig. 11
figure 12

(Top left) Time response of the platform position showing linear critically damped motion to the target. Time response of the platform orientation shows undamped oscillation due to the null space torques arising to satisfy the passivity requirement. The control gains are K x =100 and K v =20. (Top right) Time response of the control torques τ 4 and τ 6 during goal movement. (Bottom left) Zero control torque (numerical error at the order of 10−12) is produced at the passive joints, τ 1, τ 2, τ 3, and τ 5, due to the passivity condition. (Bottom right) Time response of the uncontrolled constraint forces

Full size image

3.3 Implicit conditions on the constraint forces

Rather than partition the constraint forces, we can specify a set of conditions on the constraint forces. In the case that k>p (motion actuated) and we choose to control both task and null space, we have

$$ \boldsymbol{\tau} + \boldsymbol{\Phi}^T\boldsymbol{\lambda} = \widehat{\boldsymbol{\Theta }}^T\mathbf {J}^T\bigl(\widehat{\boldsymbol{\Lambda}}_c\mathbf {f}^\star+ \widehat{\boldsymbol{\mu}}_c + \widehat{\mathbf {p}}_c\bigr) + \boldsymbol{ \Phi}^T(\widehat{\boldsymbol{\alpha}} + \widehat{\boldsymbol{\rho}}) + \widehat{\mathbf {U}}_c^T\boldsymbol{\tau}_N, $$
(68)

complemented by the following conditions on the constraints forces:

$$ \mathbf {A}(\mathbf {q},\dot{\mathbf {q}}) \boldsymbol{\lambda} = \mathbf {d}(\mathbf {q}, \dot{\mathbf {q}}), $$
(69)

where \(\mathbf {A}(\mathbf {q},\dot{\mathbf {q}}) \in \mathbb {R}^{{(k-p)} \times {m_{C}}}\) and \(\mathbf {d}(\mathbf {q},\dot{\mathbf {q}}) \in \mathbb {R}^{k-p}\), and the passivity constraints

$$ \mathbf {S}_p\, \boldsymbol{\tau} = \mathbf {0}. $$
(70)

This can be expressed as the following system of n+m C equations:

$$ \left ( { \begin{array}{c@{\quad}c} \mathbf {1} & {\boldsymbol{\Phi}^T} \\ \mathbf {0} & \mathbf {A} \\ {\mathbf {S}_p} & {\mathbf {0}} \\ \end{array} } \right )\left ( { \begin{array}{c} {\boldsymbol{\tau}} \\ {\boldsymbol{\lambda}} \\ \end{array} } \right ) = \left ( { \begin{array}{c} {\mathbf {s}(\mathbf {q},\dot{\mathbf {q}}) + \widehat{\mathbf {U}}_c^T\boldsymbol{\tau}_N} \\ {\mathbf {d}} \\ {\mathbf {0}} \\ \end{array} } \right ). $$
(71)

A block diagram of this control scheme is shown in Fig. 12.

Fig. 12
figure 13

The task-level tracking controller for a constrained system with implicit conditions on the constraint forces, A λ=d. The desired task motion and null space motion are tracked using appropriate dynamic compensation that accounts for the constraints. Simultaneously, conditions on the constraint forces are satisfied by the generated torques

Full size image

In the case that k>m (task actuated) and we choose to control only the task, we have

$$ \boldsymbol{\tau} + \boldsymbol{\Phi}^T\boldsymbol{\lambda} - \widehat{\mathbf {U}}_c^T\boldsymbol{\tau}_N = \widehat{\boldsymbol{\Theta}}^T\mathbf {J}^T\bigl(\widehat{\boldsymbol{\Lambda}}_c\mathbf {f}^\star+ \widehat{\boldsymbol{\mu}}_c + \widehat{\mathbf {p}}_c\bigr) + \boldsymbol{ \Phi}^T(\widehat{\boldsymbol{\alpha}} + \widehat{\boldsymbol{\rho}}), $$
(72)

complemented by the following conditions on the constraints forces;

$$ \mathbf {A}(\mathbf {q},\dot{\mathbf {q}}) \boldsymbol{\lambda} = \mathbf {d}(\mathbf {q}, \dot{\mathbf {q}}), $$
(73)

where \(\mathbf {A}(\mathbf {q},\dot{\mathbf {q}}) \in \mathbb {R}^{{(k-m)} \times {m_{C}}}\) and \(\mathbf {d}(\mathbf {q},\dot{\mathbf {q}}) \in \mathbb {R}^{k-m}\). The passivity constraints are

$$ \mathbf {S}_p\, \boldsymbol{\tau} = \mathbf {0}. $$
(74)

This can be expressed as the following system of n+m C +N equations:

$$ \left ( { \begin{array}{c@{\quad}c@{\quad}c} \mathbf {1} & {\boldsymbol{\Phi}^T} & - \widehat{\mathbf {U}}_c^T \\ {\mathbf {0}} & {\mathbf {A}} & {\mathbf {0}} \\ {\mathbf {S}_p} & {\mathbf {0}} & {\mathbf {0}} \\ \end{array} } \right )\left ( { \begin{array}{c} {\boldsymbol{\tau}} \\ {\boldsymbol{\lambda}} \\ {\boldsymbol{\tau}_N} \\ \end{array} } \right ) = \left ( { \begin{array}{c} {\mathbf {s}(\mathbf {q},\dot{\mathbf {q}})} \\ {\mathbf {d}} \\ {\mathbf {0}} \\ \end{array} } \right ). $$
(75)

3.3.1 Example 4

As an illustrative example we consider the humanoid torso depicted in Fig. 13 turning a valve. The system is described by n=13 generalized coordinates. The constraint equations associated with the loop closure are

$$ \boldsymbol{\phi} (\mathbf {q}) = \left ( { \begin{array}{c} \mathbf {r}_{\text{rh}} - \mathbf {r}_{\text{vrh}} \\ \mathbf {r}_{\text{lh}} - \mathbf {r}_{\text{vlh}} \\ \alpha_{\text{rh}} - \alpha_{\text{vrh}} \\ \beta_{\text{rh}} - \beta_{\text{vrh}} +\pi/2 \\ \alpha_{\text{lh}} - \alpha_{\text{vlh}} \\ \beta_{\text{lh}} - \beta_{\text{vlh}} -\pi/2\\ \end{array} } \right ), $$
(76)

where r rh and r lh are the contact locations on the left and right hand, respectively, and r vrh and r vlh are the corresponding contact locations on the valve. The terms α and β denote the xy Euler angles in an xyz sequence describing the orientation of the hands and the valve. We note that m C =10 and p=3. Considering all joints except the valve joint to be actuated, we have k=12 and

$$ \mathbf {S}_p = \left ( { \begin{array}{c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c} 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \\ \end{array} } \right ). $$
(77)
Fig. 13
figure 14

(Left) Humanoid upper body turning a valve. The system is described by n=13 generalized coordinates. Generalized coordinates q 1,…,q 12 are associated with the humanoid structure and q 13 is associated with the passive valve. Constraints, ϕ(q), are defined between the humanoid hands and the valve. In this case n=13, m C =10, p=3, m=1, N=2, and k=12. (Right) The humanoid is commanded to turn the valve while a set of conditions, A λ=d, are specified on the constraint forces. The time response of the valve angle shows linear critically damped motion to the goal. The control gains are K x =100 and K v =20. The time responses of the first six controlled constraint forces (in the global reference frame) are plotted as well

Full size image

We will define the task to control the valve angle (see Fig. 13). So, m=1, N=2, and

$$ \mathbf {x} \triangleq q_{13} = \theta. $$
(78)

We will specify the reference value as

$$ \mathbf {x}_d = 0.18 \text{ rad}. $$
(79)

The null space torque will be specified to be zero (τ N =0). Additionally, we wish to specify the first nine constraint forces. Thus,

$$ \mathbf {A} = \left ( { \begin{array}{c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c} 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 \\ \end{array} } \right ). $$
(80)

We will specify the reference value of the constraint forces as

$$ \mathbf {d} = \left ( { \begin{array}{c} Q_y(q_{13}) (-5, 1, -1)^T \\ Q_y(q_{13}) (5, 1, 1)^T \\ 0 \\ 0 \\ 0 \\ \end{array} } \right ). $$
(81)

That is, the desired constraint forces are specified in the local valve reference frame and transformed into the global frame.

The linear (PD) control law of (32) is used as the input of the decoupled system. The gains are chosen so as to achieve critically damped behavior of the task motion. Equation (71) is solved to compute the control torque. A small dissipative term has been added to the null space. Figure 13 shows simulation plots for the system under goal position commands on the task coordinate, x. The time response of the valve angle shows linear critically damped motion to the target. The time response of the first six controlled constraint forces (in the global reference frame) are plotted.

3.3.2 Constraint management

A general methodology for handling constraints will now be described that aggregates the persistent (bilateral) constraints associated with the robot mechanism and the transient (unilateral) constraints associated the interaction of the robot with the environment. In the methodology depicted in Fig. 14, a robot is interacting with the environment. At each time step, unilateral constraints associated with the robot/environment interaction are inferred from the sensed state of the interaction. These constraints are aggregated with the known internal (bilateral) mechanism constraints of the robot, and the properties associated with the constrained dynamics of the robot/environment system are computed and sent to the controller. A set of equality conditions on the robot/environment constraint forces are specified that satisfy the inequality conditions associated with maintaining desired robot/environment interactions (e.g., holding onto an object, releasing an object, maintaining contact with surfaces). These conditions are aggregated with any control conditions on the internal mechanism constraints of the robot and sent to the controller. The resulting conditions on the constraint forces, along with the specified task and null space motion commands are used to compute the control input (joint torques) to the robot.

Fig. 14
figure 15

System-level architecture for task-level constrained motion and force control. Robot/environment interaction constraints are inferred. These are appended to the internal constraints of the robot and the robot/environment dynamic properties are updated and sent to the controller. Equality conditions on the constraint forces are specified based on the constraint maintenance inequality conditions as well as on control commands for the internal robot constraint forces. These conditions are sent to the controller where, along with the specified task and null space motion commands, they are used to compute the joint torques to the robot. The system parameters generated by each block are shown based on implicit handling of constraint conditions

Full size image

Figure 15(left) depicts a simulation scenario where the humanoid robot transitions from unconstrained task-level motion control of its hands to constrained task-level motion and force control of an object that has been grappled. The methodology of Fig. 14 can be applied to this problem. Given a model of the environment the constraints associated with interacting with the grappled object would be inferred. Constraint maintenance conditions would then be specified as inequality conditions (e.g., minimum normal forces needed to be applied by the hands on the object to maintain grapple). Equality conditions on the constraint forces would then be specified to the controller, consistent with the constraint maintenance conditions. A similar methodology could be applied to a humanoid robot interacting with the environment through foot/ground contact and hand contact. Figure 15(right) depicts a simulation scenario where the humanoid robot generates a desired force at the hand (against a wall, for example) while maintaining desirable contact forces at the feet.

Fig. 15
figure 16

(Left) A humanoid robot transitions from unconstrained task-level motion control of its hands to constrained task-level motion and force control of an object that has been grappled. Given a model of the environment interaction constraints would be inferred. Constraint maintenance conditions would then be specified as inequality conditions (e.g., minimum normal forces needed to be applied by the hands on the object to maintain grapple) and equality conditions on the constraint forces would be specified to the controller. (Right) A humanoid robot generates a desired force at the hand (against a wall, for example) while maintaining desirable contact forces at the feet

Full size image

3.4 Discussion of particular limitations on control

We have to this point not discussed specific situations under which the general formulation of the control problems presented in this paper have no solutions. Specific problems can be physically ill-posed in which case rank deficiencies will exist in the system matrix. This is a recognition that care must be taken to form physically well-posed problems. For example, constraint forces cannot be specified in such a manner as to violate force equilibrium.

There are numerous ways that problems can be ill-posed. As a simple example, consider two unit point masses, A and B, moving on a plane described by

$$ \boldsymbol{\tau} = \mathbf {M}\ddot{\mathbf {q}}, $$
(82)

where

$$ \mathbf {q} = \left ( { \begin{array}{c@{\quad}c@{\quad}c@{\quad}c} x_\text{A} & y_\text{A} & x_\text{B} & y_\text{B} \\ \end{array} } \right )^T, \quad\quad \boldsymbol{\tau} = \left ( { \begin{array}{c@{\quad}c@{\quad}c@{\quad}c} f_{x_\text{A}} & f_{y_\text{A}} & f_{x_\text{B}} & f_{y_\text{B}} \\ \end{array} } \right )^T, $$
(83)

and

$$ \mathbf {M} = \left ( { \begin{array}{c@{\quad}c@{\quad}c@{\quad}c} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{array} } \right ). $$
(84)

Let us define the task to control only the second point mass

$$ \mathbf {x} \triangleq \left ( { \begin{array}{c@{\quad}c} x_\text{B} & y_\text{B} \\ \end{array} } \right )^T. $$
(85)

The task Jacobian is then

$$ \mathbf {J} = \left ( { \begin{array}{c@{\quad}c@{\quad}c@{\quad}c} 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{array} } \right ). $$
(86)

Considering only the first point mass to be to be actuated, we have n=4, m C =0, p=4, m=2, N=2, k=2, and

$$ \mathbf {S}_p = \left ( { \begin{array}{c@{\quad}c@{\quad}c@{\quad}c} 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{array} } \right ). $$
(87)

Our task space parameters are

$$ \boldsymbol{\Lambda} = \bigl(\mathbf {J}\mathbf {M}^{-1}\mathbf {J}^T \bigr)^{-1} = \left ( { \begin{array}{c@{\quad}c} 1 & 0 \\ 0 & 1 \\ \end{array} } \right ), $$
(88)

and

$$ \mathbf {N}^T = \mathbf {1} - \mathbf {J}^T\boldsymbol{\Lambda} \mathbf {J}\mathbf {M}^{-1} = \left ( { \begin{array}{c@{\quad}c@{\quad}c@{\quad}c} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ \end{array} } \right ), $$
(89)

or in terms of an N dimensional basis,

$$ \mathbf {U}^T = \left ( { \begin{array}{c@{\quad}c} 1 & 0 \\ 0 & 1 \\ 0 & 0 \\ 0 & 0 \\ \end{array} } \right ). $$
(90)

We have the following system of equations:

$$ \left ( { \begin{array}{c@{\quad}c} \mathbf {1} & - \mathbf {U}^T \\ {\mathbf {S}_p} & {\mathbf {0}} \\ \end{array} } \right )\left ( { \begin{array}{c} {\boldsymbol{\tau}} \\ {\boldsymbol{\tau}_N} \\ \end{array} } \right ) = \left ( { \begin{array}{c} {\mathbf {J}^T\boldsymbol{\Lambda}\ddot{\mathbf {x}}} \\ {\mathbf {0}} \\ \end{array} } \right ). $$
(91)

However, in this case,

$$ \left ( { \begin{array}{c@{\quad}c} \mathbf {1} & - \mathbf {U}^T \\ {\mathbf {S}_p} & {\mathbf {0}} \\ \end{array} } \right ) = \left ( { \begin{array}{c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c} 1 & 0 & 0 & 0 & -1 & 0 \\ 0 & 1 & 0 & 0 & 0 & -1 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ \end{array} } \right ) $$
(92)

is rank deficient (rank is 4) and the control problem cannot be solved. Hence, it is not possible to control point mass B with actuation only at point mass A, where the point masses are decoupled. If, however, we couple the point masses such that point mass B is physically constrained to move with point mass A then n=4, m C =2, p=2, m=2, N=0, k=2, and

$$ \boldsymbol{\Phi} = \left ( { \begin{array}{c@{\quad}c@{\quad}c@{\quad}c} 1 & 0 & -1 & 0 \\ 0 & 1 & 0 & -1 \\ \end{array} } \right ). $$
(93)

We note that

$$ \boldsymbol{\Theta}^T = \mathbf {1} - \boldsymbol{\Phi}^T \bar{\boldsymbol{\Phi}}^T = \left ( { \begin{array}{c@{\quad}c@{\quad}c@{\quad}c} \frac{1}{2} & 0 & \frac{1}{2} & 0 \\ 0 & \frac{1}{2} & 0 & \frac{1}{2} \\ \frac{1}{2} & 0 & \frac{1}{2} & 0 \\ 0 & \frac{1}{2} & 0 & \frac{1}{2} \\ \end{array} } \right ). $$
(94)

We then have

$$ \boldsymbol{\Lambda}_c = \bigl(\mathbf {J}\mathbf {M}^{- 1} \boldsymbol{\Theta}^T \mathbf {J}^T\bigr)^{- 1} = \left ( { \begin{array}{c@{\quad}c} 2 & 0 \\ 0 & 2 \\ \end{array} } \right ). $$
(95)

The null space term \(\mathbf {N}_{c}^{T}\) vanishes, and we have

$$ \left ( { \begin{array}{c@{\quad}c} \mathbf {1} & {\boldsymbol{\Phi}^T} \\ {\mathbf {S}_p} & {\mathbf {0}} \\ \end{array} } \right )\left ( { \begin{array}{c} {\boldsymbol{\tau}} \\ {\boldsymbol{\lambda}} \\ \end{array} } \right ) = \left ( { \begin{array}{c} \boldsymbol{\Theta}^T\mathbf {J}^T\boldsymbol{\Lambda}_c\ddot{\mathbf {x}} \\ {\mathbf {0}} \\ \end{array} } \right ). $$
(96)

In this case, the term

$$ \left ( { \begin{array}{c@{\quad}c} \mathbf {1} & {\boldsymbol{\Phi}^T} \\ {\mathbf {S}_p} & {\mathbf {0}} \\ \end{array} } \right ) = \left ( { \begin{array}{c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c} 1 & 0 & 0 & 0 & 1 & 0 \\ 0 & 1 & 0 & 0 & 0 & 1 \\ 0 & 0 & 1 & 0 & -1 & 0 \\ 0 & 0 & 0 & 1 & 0 & -1 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ \end{array} } \right ) $$
(97)

is of full rank and the control has a solution. For example, given desired unit accelerations of point mass B in x and y, that is, \(\ddot{\mathbf {x}} = \left ( { \begin{array}{c@{\quad}c} 1 & 1 \\ \end{array} } \right )\), we have

$$ \left ( { \begin{array}{c} {\boldsymbol{\tau}} \\ {\boldsymbol{\lambda}} \\ \end{array} } \right ) = \left ( { \begin{array}{c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c} f_{x_\text{A}} & f_{y_\text{A}} & f_{x_\text{B}} & f_{y_\text{B}} & \lambda_1 & \lambda_1 \\ \end{array} } \right )^T = \left ( { \begin{array}{c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c} 2 & 2 & 0 & 0 & -1 & -1 \\ \end{array} } \right )^T. $$
(98)

So, without any actuation forces at point mass B it can be accelerated due to the constraint coupling with point mass A. Point mass A has actuator forces of 2 applied in x and y to generate unit accelerations at point mass B. Constraint forces of −1 occur between point mass A and B. This is a trivial example as the constraints effectively lump the two unit point masses together into a single point mass; nevertheless, it is instructive in examining the solvability of the control equations.

We now consider the more complex example of a bimanual robot holding an object as depicted in Figs. 15 and 16. The robot possess 12 fully actuated generalized coordinates. The object is described by an additional 6 generalized coordinates. Our task is to control the object in 6 dimensions,

$$ \mathbf {x} \triangleq \left ( { \begin{array}{c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c} x_\text{obj} & y_\text{obj} & z_\text{obj} & \alpha_\text{obj} & \beta _\text{obj} & \gamma_\text{obj}\\ \end{array} } \right )^T. $$
(99)

The grasping constraints are expressed as

$$ \boldsymbol{\phi} (\mathbf {q}) = \left ( { \begin{array}{c} \mathbf {r}_{\text{rh}} - \mathbf {r}_{\text{orh}} \\ \mathbf {r}_{\text{lh}} - \mathbf {r}_{\text{olh}} \\ \alpha_{\text{rh}} - \alpha_{\text{orh}} \\ \beta_{\text{rh}} - \beta_{\text{orh}} +\pi\\ \gamma_{\text{rh}} - \gamma_{\text{orh}} \\ \alpha_{\text{lh}} - \alpha_{\text{olh}} \\ \beta_{\text{lh}} - \beta_{\text{olh}}\\ \gamma_{\text{lh}} - \gamma_{\text{olh}} \\ \end{array} } \right ). $$
(100)

We then have n=18, m C =12, p=6, m=6, N=0, k=12, where we will control the task as well as the normal hand constraint forces on the object and 4 of the hand constraint moments on the object, so

$$ \mathbf {A} = \left ( { \begin{array}{c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c} 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 \\ \end{array} } \right ), \quad\quad \mathbf {d} = \left ( { \begin{array}{c} \lambda_1 \\ \lambda_4 \\ 0 \\ 0 \\ 0 \\ 0 \\ \end{array} } \right ). $$
(101)

Note we have chosen to specify zero constraint moments. Our system can be expressed as

$$ \left ( { \begin{array}{c@{\quad}c} \mathbf {1} & {\boldsymbol{\Phi}^T} \\ {\mathbf {0}} & {\mathbf {A}} \\ {\mathbf {S}_p} & {\mathbf {0}} \\ \end{array} } \right )\left ( { \begin{array}{c} {\boldsymbol{\tau}} \\ {\boldsymbol{\lambda}} \\ \end{array} } \right ) = \left ( { \begin{array}{c@{\quad}c} \widehat{\boldsymbol{\Theta}}^T\mathbf {J}^T(\widehat{\boldsymbol{\Lambda}}_c\ddot{\mathbf {x}} + \widehat{\boldsymbol{\mu}}_c + \widehat{\mathbf {p}}_c) + \boldsymbol{\Phi}^T(\widehat{\boldsymbol{\alpha}} + \widehat{\boldsymbol{\rho}}) \\ {\mathbf {d}} \\ {\mathbf {0}} \\ \end{array} } \right ), $$
(102)

where

$$ \left ( { \begin{array}{c@{\quad}c} \mathbf {1} & {\boldsymbol{\Phi}^T} \\ {\mathbf {0}} & {\mathbf {A}} \\ {\mathbf {S}_p} & {\mathbf {0}} \\ \end{array} } \right ) \in \mathbb {R}^{{30} \times {30}}. $$
(103)

For the grasping posture shown in Fig. 16,

$$ \text{Rank} \left [\left ( { \begin{array}{c@{\quad}c} \mathbf {1} & {\boldsymbol{\Phi}^T} \\ {\mathbf {0}} & {\mathbf {A}} \\ {\mathbf {S}_p} & {\mathbf {0}} \\ \end{array} } \right ) \right ] = 29. $$
(104)

So, the system is rank deficient. However, for particular choices of \(\ddot{\mathbf {x}}\), λ 1, and λ 4 control solutions can be found. For example, given \(\ddot{\mathbf {x}}= \mathbf {0}\) (static equilibrium of the object) the condition λ 1=−λ 4 produces a solution. That is, the vector

$$ \left ( { \begin{array}{c} \widehat{\boldsymbol{\Theta}}^T\mathbf {J}^T(\widehat{\boldsymbol{\Lambda}}_c\ddot{\mathbf {x}}+ \widehat{\boldsymbol{\mu}}_c + \widehat{\mathbf {p}}_c) + \boldsymbol{\Phi}^T(\widehat{\boldsymbol{\alpha}} + \widehat{\boldsymbol{\rho}}) \\ {\mathbf {d}} \\ {\mathbf {0}} \\ \end{array} } \right ) $$
(105)

can be expressed as a linear combination of the columns of

$$ \left ( { \begin{array}{c@{\quad}c} \mathbf {1} & {\boldsymbol{\Phi}^T} \\ {\mathbf {0}} & {\mathbf {A}} \\ {\mathbf {S}_p} & {\mathbf {0}} \\ \end{array} } \right ), $$
(106)

when \(\ddot{\mathbf {x}}= \mathbf {0}\) and λ 1=−λ 4. However, (105) cannot be expressed as a linear combination of the columns of (106) when \(\ddot{\mathbf {x}}= \mathbf {0}\) and λ 1≠−λ 4. This is an intuitive result since the constraint forces applied by the left and right hands on the object cannot be arbitrary but must satisfy force equilibrium.

Fig. 16
figure 17

Care must be taken so as not to arbitrarily specify constraint forces in such a manner as to violate force equilibrium. In this example, normal constraint forces applied by the robot hands against the object need to balance under static equilibrium

Full size image

4 Conclusion

A novel approach has been presented for formulating motion control for holonomically constrained multibody systems that allows for the simultaneous specification of desired constraint forces. The approach presented is based on a decomposition of task, constraint, and posture space, and it leverages the symmetry between constrained dynamics and task space dynamics. It provides a natural scheme for control synthesis by exposing the coordinates, both motion and constraint, of interest. The approach can be realized through a partitioning of the constraint forces or through the specification of implicit conditions on the constraint forces. The necessary conditions relating the number of actuators, motion coordinates, and constraint coordinates have been described. The examples presented indicate that the analytical framework can be implemented in practical constrained multibody control problems. A system-level approach for constraint management during robot interactions with the environment has also been presented.

As a practical matter it is assumed that the controller has access to the system state (via a forward dynamics solver in the simulated case or via sensors in the physical case) and estimates of the dynamic properties of the physical system. It should also be noted that the system of equations described by (41), (60), (71), and (75) may not always be well conditioned. That is, the system matrix in these equations may not always be invertible. Even given a satisfaction of the conditions relating the number of actuators, motion coordinates, and constraint coordinates; constraint forces may not always be arbitrarily specified. For example, given a bimanual robot holding an object (see Fig. 15), the vertical constraint forces applied to the object would need to balance the weight of the object and could not be arbitrarily specified under static equilibrium. Additionally, the normal constraint forces applied by the robot hands against the object would need to balance under static equilibrium as discussed in the previous section.