Case studies for computed torque control of constrained underactuated systems

Computed torque control (CTC) method is an efficient technique for trajectory tracking control of robot manipulators. As a model-based control, CTC needs the inverse dynamics calculation of the dynamical system. A special group of these systems is formed by the underactuated ones, in which the number of independent control inputs is lower than the degrees of freedom of the system. In these systems, the inverse dynamics calculation is a challenging task, because the inverse calculation leads to the solution of a differential-algebraic equation (DAE). Complex robotic structures like the ones with closed kinematic loops are generally modeled by redundant descriptor coordinates instead of the Lagrangian approach when a minimum set of generalized coordinates are chosen. The use of non-minimum set of descriptor coordinates requires the introduction of a set of geometric constraints, which are expressed in the form of algebraic equations. Thus, the mathematical model of the robotic structure itself is also a DAE. A
few different CTC method based algorithms are studied. The control algorithms are compared in the case of the simplest possible linear dynamical system with special attention to the choice of the descriptor coordinate set.


Introduction
One of the most common ways of controlling robot motion is based on a linear control system obtained by feeding back the dynamics of the original non-linear system.After this feedback linearization, an arbitrary motion can be prescribed, which is realizable until the actuators are able to provide the required torques.This method cannot be directly applied in case of the so-called underactuated systems because the number of the independent control inputs is lower than the degrees of freedom (DoF) of the system.For a general overview of this problem let us consider the following equation of motion: q = f 1 (q, q, t) + f 2 (q, q, t)u, where q is the vector of the generalized coordinates of minimum number and u is the control input vector.The only assumption in the general equation of motion (1) is that the control input u appears linearly in the system.The system is fully actuated, if the rank of the input matrix f 2 (q, q, t) equals to the DoFs of the system: rank(f 2 (q, q, t)) = dim(q).
In case of such a fully actuated system the control input can be formulated by inverting the control input matrix f 2 (q, q, t) as: u = f 2 (q, q, t) −1 −f 1 (q, q, t) + ũ .
If we substitute (3) into the equation of motion (1), we obtain the following linear system: where the synthetic control input ũ can be chosen, for example, as ũ = qd + K D qd − q + K P q d − q , with superscript d referring to the desired values.Here, K D and K P are positive definite gain matrices.If we calculate the control input u according to (3) and we can measure q and q exactly than the acceleration of the system can be prescribed arbitrarily via the synthetic control input ũ.For the above explained method vector f 1 (q, q, t) and matrix f 2 (q, q, t) has to be known exactly.
The above explained computed torque method cannot be accomplished in case of underactuated systems, because f 2 (q, q, t) is not invertible in that case.We speak about underactuated systems if the number of the independent control inputs is lower than the DoFs of the system [1,2], thus rank(f 2 (q, q, t)) < dim(q).(6) In spite of the difficulties of controlling underactuated systems, the acquired knowledge lets us design and build more energy efficient and more agile robots.Cranes are typically underactuated systems because there is no direct influence of the actuators on the swinging payload.The motion of the top mounting point of the cable can be controlled directly, but the motion of the payload is defined by the dynamics of the system.A lot of different approaches were developed in the past for the anti-sway control of the cranes [3,4].Some newly designed robotic systems utilize the advantages of the underactuation, like agile motion and energy efficient operation.The domestic robot called ACROBOTER [5] hangs down from the ceiling on a suspending cable similarly to cranes, and it is able to utilize the pendulum-like motion efficiently.In practice, also the flexibility of the machine components can cause underactuation [6,7].Typical examples are rotors driven via a flexible shafts, or lightweight robots with flexible arms.
An important step of the control design of robotic manipulators is the choice of the generalized or descriptor coordinates.The dynamical modeling of mechanically complex robotic structures via the use of minimum number generalized coordinates can be inefficient.A more effective way is to choose a non-minimum set of descriptor coordinates (redundant or dependent ones) of number larger than the DoFs and to introduce appropriate geometric constraints in the form of algebraic equations.Several techniques are available in the literature, e.g., the use of reference point coordinates, natural coordinates or mixed coordinates [8].The numerical computation is more effective with the use of non-minimum set descriptor coordinates, however the dynamical investigation is more difficult because of the presence of the algebraic equations.
In this study, we investigate the computed torque control (CTC) of underactuated systems described by non-minimum set of coordinates via a case study which is as simple as possible but catches the main character of such systems.

Dynamics of underactuated systems
In general a robotic manipulator system as well as any controlled mechanical system can be described by the following equation of motion using minimum set generalized coordinates: where M(q) ∈ R n×n is the positive definite mass matrix, C(q, q) ∈ R n is the vector of forces arising partly from the dynamics of the system (Coriolis, centrifugal, etc.) and from active forces (springs, dampers, etc.).Q(q) ∈ R n is the vector of gravitational forces.H(q) ∈ R n×l is the control input matrix and u ∈ R l is the control input vector.Many studies like [6,[9][10][11][12][13] assume that it is possible to decompose the generalized coordinates q into active (actuated) q a and passive (unactuated) q p coordinates leading to: This decomposition is directly possible if the assumption stands [11].However, the full state feedback linearization cannot be carried out for (8), because of the lack of control in the second part of the equations.This second part forms a second order non-holonomic constraint in the system.From the viewpoint of the inverse dynamics calculation we can conclude that ( 8) is a differential-algebraic equation (DAE), because the control input u appears as an algebraic variable additionally to q.
In general cases when the assumption ( 9) is not satisfied, we can transform the system (7) into a form similar to (8) via the projection of the system into the null-space of the input matrix H(q).Let us consider the null-space projection matrix V(q) ∈ R (n−l)×n of H(q) as: With (10), the n − l dimensional passive part of the equation of motion (7), also named as internal-dynamics of the system, can be reformulated as: V(q)M(q) q + V(q)C(q, q) − V(q)Q(q) = 0.
If we apply the idea of the Moore-Penrose pseudo-inverse then the l dimensional active part of the equation of motion (7) can also be derived in the form: where it can be shown in algebraic way that the H † (q) ∈ R l×n Moore-Penrose pseudo-inverse of the input matrix can be calculated as: The split into active and passive parts of the equation of motion is even more difficult in the presence of geometric constraint equations, when mechanically complex robotic structures are modeled by non-minimum set of descriptor coordinates.A possible solution is to project the equation of motion into the subspace of kinematically possible motions [14].After this projection, we end up with an equation of motion of the form of (7) and any control technique developed for underactuated systems (e.g.partial feedback linearization [10,11,15], computed desired computed torque control [6,9]) can be applied.However the sequence of projections may require high computational effort.Consequently the other possibility is to apply the control algorithm directly for the DAE form of the system, which will be detailed in the subsequent sections.

Problem formulation
In the present work, we study the CTC method applied to underactuated systems described geometrically by non-minimum set of descriptor coordinates (dependent coordinates).In case of non-minimum number of descriptor coordinates, geometric constraint equations provide the connection between the redundant descriptor coordinates.Thus, the equation of motion of such systems is given in DAE form.Instead of transforming the equation of motion to ODE, CTC algorithms can directly be applied for the DAE system, so the CTC method for underactuated systems can be generalized for systems modeled by non-minimum set of coordinates.
Using non-minimum set of descriptor coordinates the dynamical model can be written in the form of a differential-algebraic equation, which has the following general form [1,8]: where the positive definite mass matrix M(q) ∈ R n×n is constant in case of the use of natural coordinates [8].However, we do not focus on this special case, thus, in general the mass matrix may depend on the descriptor coordinates.Vector C(q, q) ∈ R n is the vector of the forces arising from the dynamics of the system.The matrix ϕ ϕ ϕ q (q) = ∂ϕ ϕ ϕ(q)/∂q ∈ R m×n is the constraint Jacobian associated with the geometric constraints ϕ ϕ ϕ(q) ∈ R m .For the sake of simplicity, the geometric constraints are considered to be scleronomic, but any deduction in this work can be generalized for explicitly time dependent geometric constraints, too.Q(q) ∈ R n is the vector of gravitational forces.H(q) ∈ R n×l is the control input matrix and u ∈ R l is the control input vector.We assume that the system is underactuated, so the dimension of the control input l is lower than the DoFs n − m.
The inverse kinematical and dynamical calculations may lead to a unique solution if the number of control inputs and the dimension of the task is equal [7,16,17].In order to reach this, we assume that the task is defined by l number of algebraic equations.This set of additional constraint equations are the so-called servo-constraints (also named control-constraints): where σ σ σ(q, t) ∈ R l .The servo-constraint vector can be handled similarly to the geometric constraints (15), however the servoconstraints usually depend on time explicitly, thus they are rheonomic.We assume that the servo constraints and the geometric constraints are linearly independent and consistent, and the servo constraints can be satisfied with bounded control forces.
In some special cases, we can also assume that these servoconstraints can be written in the following form: where g(q) represents, for example, the end-effector position of the robot as the function of the descriptor coordinates and p(t) is an arbitrarily prescribed function of time expressing the performance goal to be realized [7].
With the loss of generality, reference [9] introduces the notion of the the so-called controlled and uncontrolled coordinates.In the simplest case we can say that those coordinates whose trajectories are prescribed in time via the task definition are the controlled coordinates.The uncontrolled ones must be calculated with respect to the dynamics of the system, and there is no direct restriction for them from the side of the task description of the manipulator.In general, we cannot say that there is a set of coordinates which are prescribed by the task.Still, in some cases, the servo-constraints and a well chosen subset of geometric constraints can be solved for the controlled coordinates q c in closed form [17].However, the choice of the controlled coordinates is not always unique and in case of complex systems, the choice is not obvious.Additionally we mention that the choice of the descriptor coordinates influences the above method.Finally, if we can use the notion of the controlled coordinates, then the task can be defined as where the superscript d refers to the desired trajectory.In this formulation, the controlled coordinates are prescribed functions of time.In such case, the descriptor coordinates can be split into controlled and uncontrolled parts as: respectively, where S c and S u are task dependent selector matrices.This simplification can be utilized for saving computational time.
Reference [9] introduces the notion of collocated and noncollocated cases of the control of underactuated systems.These terms are in connection with the separation into controlled coordinates q c and uncontrolled coordinates q u .Besides, let us consider the separation between active (actuated) and passive (unactuated) coordinates denoted by q a and q p , respectively.Reference [9] defines the collocated case by which means that the controlled coordinates are the actuated ones.In the so-called non-collocated case which means that the controlled coordinates are the passive ones.An important message of [9] is that the partial feedback linearization and the CTC cannot be done in the non-collocated case when there is no inertial coupling between the controlled and the active coordinates.In this study, we point out the importance of the choice of the descriptor coordinates, because this choice can effect the inertial coupling between the coordinates.Note that the collocated and non-collocated cases are very special ones, because in general, the actuation of the coordinates depends on the mechanical design of the manipulator while the separation between controlled and uncontrolled coordinates depends on the control task; in general, they are not in connection to each other.Besides, the separation between controlled and uncontrolled coordinates is not straightforward.

The investigated mechanical model
The aim of this work is to investigate the CTC method in case of the simplest non-trivial dynamical system having the properties described in the previous section.Consequently, we consider an underactuated n − m = 2 DoF linear system with l = 1 control input only.We also introduce m = 1 geometric constraint equation, so the number of descriptor coordinates has to be n = 3.The model shown in Fig. 1 is actuated by l = 1 control force F, and the single geometric constraint is represented by a rigid rod of length l 23 .
The equation of motion formulated in (14,15) can be simplified with the stiffness (K) and the damping (D) matrices: One has several options to choose descriptor coordinates.The most trivial way is to choose the absolute Cartesian coordinates of the blocks of masses m 1 , m 2 and m 3 as it can be seen in Fig. 1: The mass, the stiffness and the damping matrices are respectively.The geometric constraint and the constraint Jacobian are: The generalized force vector due to the gravity is The coefficient of the control input and the control input itself are: respectively.
As already emphasized in the previous section, the applicability of the control approaches highly depend on the chosen descriptor coordinate set.To represent this, we also choose relative coordinates, which are also typical in robotics, since these are measured by the encoders.Introduce the descriptor coordinate vector q = [z 1 , ẑ2 , ẑ3 ] T , where the relative coordinates can be calculated from the previously introduced absolute ones as ẑ2 = z 2 − z 1 and ẑ3 = z 3 − z 2 .The new form of the system matrices are the following: The most important difference between the two descriptions is that the absolute coordinates do not present inertial coupling between the coordinates due to the diagonal mass matrix, while inertial coupling is present in case of relative coordinates.These trivial observations become especially important because reference [9] shows that the CTC in non-collocated cases is possible only if there is inertial coupling between the controlled and the active coordinates.

Control approaches
In this section, the method based on the backward Euler (BE) direct discretization of the control law [16] and the application of the method of Lagrange multipliers (MLM) [8] are overviewed.For the application of most of the control algorithms, the constraining forces, in mathematical terms the Lagrange multipliers, have to be eliminated from the equation of motion (14,15).For the approaches presented below, this is not needed.
For the presented methods, the formulation of the geometric constraint equation ( 15) and the servo-constraint equation ( 16) are needed at the level of acceleration by differentiating them twice with respect to time, in order to make the acceleration q appear explicitly: ϕ ϕ ϕ q (q) q + φ ϕ ϕ q (q, q) q = 0, σ σ σ q (q, t) q + σ σ σ q (q, q, t) q + ċ(q, q, t) = 0, where σ σ σ q (q) ∈ R l×n is the Jacobian of the servo-constraint and c(t) ∈ R l is the time derivative of the explicitly time dependent part of the servo-constraint.In the application of the method of Lagrange multipliers the geometric constraint equations can be stabilized by the Baumgarte method [8,18].Similarly, we extend the acceleration level servo-constraint equation (43) as follows: σ σ σ q (q, t) q + σ σ σ q (q, q, t) q + ċ(q, q, t)+ K D [σ σ σ q (q, t) q + c(q, t)] where K P ∈ R l×l and K D ∈ R l×l are positive definite gain matrices.Note that the method can be simplified if these gain matrices are substituted by scalar parameters K P and K D as it appears in the original work of Baumgarte [18].Equation ( 44) is asymptotically stable for fixed desired positions, which is utilized in the control methods presented in this section.The methods introduced in the following subsections calculate both the control input and the desired coordinates as it is done by the CDCTC method summarized in reference [6,9].

Control via direct backward discretization
As opposed to the CDCTC method, we directly apply the backward Euler discretization of the DAE system (14,15,16) and the resulting set of nonlinear algebraic equations are solved by the Newton-Raphson method for the desired actuator forces, uncontrolled coordinates and Lagrange multipliers in the upcoming time step [16,17].
First, we transform the unconstrained dynamic equation ( 14) into first order system via introducing the new variable y = q.Then we consider the geometric constraint equation ( 15) and the stabilized second time derivative (44) of the servo-constraint equation (16).After that the control law can be obtained for u from: q = y, (45) ϕ ϕ ϕ(q) = 0, (47) σ σ σ q (q, t)ẏ + σ σ σ q (q, y, t)y + ċ(q, y, t)+ K D [σ σ σ q (q, t)y + c(q, t)] + K P σ σ σ(q, t) = 0. (48) Applying the backward Euler discretization to (45-48) a 2n−l+m dimensional system of nonlinear algebraic equations arises in the form: for the i-th value of the desired coordinates q i , their time derivatives y i , the control inputs u i and the Lagrange multipliers λ λ λ i .It can be formulated as a function F(z i ) of the vector of unknowns z i : The system (49-52) can be solved by the Newton-Raphson iteration.The j-th approximation of this iteration for the unknowns in the i-th time step can be formulated as: where J(z j−1 i ) is the Jacobian of F(z j−1 i ).This Newton-Raphson iteration gives accurate result in a few steps, because the initial estimation z 0 i comes partly from the solution u i−1 and λ λ λ i−1 calculated in the previous time step and the previously sampled measurement data q i−1 and y i−1 .
The calculation of the Jacobian can be accomplished analytically and also numerically.In order to save computational time it is enough to calculate the Jacobian once in each time step, and after that the Newton-Raphson iteration uses the same Jacobian.
In some cases the Jacobian matrix may be ill-conditioned, but the problem can be handled by singular value decomposition.

Control by means of the extended method of Lagrange multipliers
The method of Lagrange multipliers (MLM) is well known regarding the numerical integration of the governing differentialalgebraic equation of multibody systems [8].In this section, the MLM is generalized for the servo-constraint based control of underactuated dynamical systems described by non-minimum set of descriptor coordinates and additional geometric constraint equations [19].
The basic idea of the method is to join the unconstrained dynamical equation ( 14) to the geometric constraint and to the servo-constraint in acceleration level given in (42) and (44).We describe two versions of this method, first the two-step MLM, and secondly the one-step MLM.These two methods differ only in numerical computation issues.
In the first step of the two-step MLM, the null-space projection matrix V(q) of the input matrix H(q) is calculated as in (10) with which the control input u can be eliminated from the unconstrained dynamic equation (14).Thus, we obtain the internal dynamics (also named uncontrolled dynamics or zero dynamics) of the system.The internal dynamics extended with the servoconstraints is expressed in the following hyper matrix form: where f k (q, t) = K D [σ σ σ q (q, t) q+c(q, t)]−K P σ σ σ(q, t).These equations can be solved for the second derivatives of the descriptor coordinates and for the Lagrange multipliers, where q and q come from the measured values.As a second step of the method, the pseudo-inverse of H(q) is calculated with which the control input can be given as u = H † (q) M(q) q + C(q, q) + ϕ ϕ ϕ T q (q)λ λ λ − Q(q) , where H † (q) is the Moore-Penrose pseudo-inverse of the control input matrix calculated as equation (13) shows.
In the one-step MLM, the unconstrained dynamic equation ( 14), the acceleration level geometric constraint equation ( 42) and the acceleration level servo-constraint equation (44) can be incorporated in hyper-matrix form as follows: − φ ϕ ϕ q (q, q) q − σ σ σ q (q, q, t) q − ċ(q, q, t) − f k (q, t) from which the control input u, the acceleration q and the vector of Lagrange multipliers λ λ λ can be calculated as the function of the measured state q and q of the system.It has to be noticed that in the case of the so-called non-collocated systems one must be aware of singularity problems related to the coefficient hypermatrix of the unknowns q, λ λ λ and u.

Comparison of the introduced control strategies
In this section, we give a numerical comparison of the introduced control algorithm for the benchmark problem presented in Fig. 1.The different test cases and their evaluations are summarized in table 1 and 2. The tested algorithms were the direct backward Euler discretization method (see section 5.1) and the 2-step and 1-step methods of Lagrange multipliers (see section 5.2).There were two different mathematical descriptions of the benchmark system: the case when the absolute Cartesian coordinates are used (Tab.1), and the case when the relative coordinates are used (Tab.2), as it is explained in details in section 4.
Additionally, all numerical simulations are carried out for four tasks.In the first task, the position of mass m 1 is prescribed by z d (t).The mathematical formulation of the servo-constraint is the same in the case of absolute and relative coordinates: Since the servo constraint can be solved for the coordinate of m 1 and the actuator force is also applied to m 1 , this is a collocated case.
Similarly, the non-collocated case can also be formulated when the prescribed trajectory is related to the "passive" DoFs represented by m 2 and m 3 .When the position of m 2 is prescribed, the servo-constraint has the forms in absolute and relative coordinate sets respectively.Call this case "non-collocated type 1".Similarly, when the position of mass m 3 is prescribed ("non-collocated type 2") we can write: Finally, a general case is considered, when the servo-constraint is in relation with both the "active" and "passive" DoFs.The task is to keep the average position of mass m 1 and m 2 on the prescribed trajectory z d (t).
Now, there is no unique solution for the servo-constraint, which means that there is no unique choice for controlled and uncontrolled coordinates.
Tab. 1 and 2 show the summary of the results of numerical simulations.Three qualitatively different behaviors of the controlled systems are distinguished.In the cases denoted by "A", the controller drives the system into the desired state without any problem, thus the servo-constraint is driven to zero even in the presence of an initial perturbation.In the cases denoted by "C", the controller encounters singularity problems, and it does not work.This problem appears only in the 1-step MLM method, when the coefficient matrix of the unknowns becomes singular (see equation (57)).
In cases denoted by "B", the control algorithm seems to work without numerical problems, but the servo-constraint is not driven to zero, consequently, the control is unsuccessful.This occurs in case of the 2-step MLM method.This method is more robust against singularity problems than the 1-step MLM, because it utilizes null-space projection and pseudo-inverse calculation instead of inverse calculation, as shown in (55) and (56).In the cases "B", equation (55) gives zero acceleration for mass m 1 , therefore the computed control force is constant.Because of the null space projection, the controller loses the information about the desired motion of the "passive" DoF.
We can conclude that the direct discretization method is successful in all test cases, even in the so-called non-collocated cases.The 2-step MLM is applicable in some cases when the 1-step MLM fails.We can also observe that the change of the descriptor coordinates from absolute to relative ones influences the operation of the control algorithm in two cases: for the noncollocated case the 2-step MLM works successfully with relative coordinates.Note that the operation of the control algorithms is the same in type 1 and type 2 non-collocated cases.
Typical trajectories, and the time history of the servoconstraints and control inputs are presented in Fig. 2, where the left column shows a simulation "A" for a non-collocated 2-step MLM relative coordinate case, and the right column shows a simulation "B" for a non-collocated 2-step MLM absolute coordinate case.

Conclusion
The computed torque control of underactuated systems is a challenging task especially if the controlled mechanical system includes closed kinematic loops described by non-minimum set of coordinates.In the present study, we numerically investigated three different computed torque control algorithms for these systems with special attention to the choice of the descriptor coordinates of the controlled mechanical system.
All the introduced control algorithms used the servoconstraint on acceleration level with Baumgarte stabilization, which is widely used in numerical simulation of multibody systems.In the cases of successful control strategies, the simulations showed that the servo-constraints were driven to zero.The detailed study of the several combinations of the choices of system coordinates, tasks and control algorithms showed that the proper choice of the descriptor coordinates may result successful control while the same control algorithm may fail with other choices of coordinates.The numerical simulations also showed the robustness of the backward Euler direct discretization of the control law, which works even for non-collocated systems with any choice of coordinates.

Fig. 1 .
Fig. 1.Underactuated and constrained mechanical model for CTC case study