University of Birmingham Operational Space Control of Constrained and Underactuated Systems

—The operational space formulation (Khatib, 1987), applied to rigid-body manipulators, describes how to decouple task-space and null-space dynamics, and write control equations that correspond only to forces at the end-effector or, alternatively, only to motion within the null-space. We would like to apply this useful theory to modern humanoids and other legged systems, for manipulation or similar tasks, however these systems present additional challenges due to their underactuated ﬂoating bases and contact states that can dynamically change. In recent work, Sentis et al. derived controllers for such systems by implementing a task Jacobian projected into a space consistent with the supporting constraints and underactuation (the so called support consistent reduced Jacobian ). Here, we take a new approach to derive operational space controllers for constrained underactuated systems, by ﬁrst considering the operational space dynamics within projected inverse-dynamics (Aghili, 2005), and subsequently resolving underactuation through the addition of dynamically consistent control torques. Doing so results in a simpliﬁed control solution compared with previous results, and importantly yields several new insights into the underlying problem of operational space control in constrained environments: 1) Underactuated systems, such as humanoid robots, cannot in general completely decouple task and null-space dynamics. However, 2) there may exist an inﬁnite number of control solutions to realize desired task-space dynamics, and 3) these solutions involve the addition of dynamically consistent null-space motion or constraint forces (or combinations of both). In light of these ﬁndings, we present several possible control solutions, with varying optimization criteria, and highlight some of their practical consequences.


I. INTRODUCTION
Dynamics of a robot are important to consider for planning and executing dexterous, fast, and compliant motions. More specifically, the inertial and energy characteristics of the endeffector, or operational degrees-of-freedom (DOFs), are most relevant for understanding the performance and behavior of tasks such as object manipulation. With this is mind, Khatib [4] presented the operational space formulation, which derives end-effector dynamics for rigid-body robot manipulators. Additionally, the resulting control solutions are able to compensate for (linearize) task dynamics and decouple them from redundant null space dynamics, such that the torques in null space are dynamically consistent with the task.
We would like to apply this powerful technique for analysis and control of a wider range of robotic systems than classical manipulators. For example, modern humanoid robots are generally complex, high DOF systems, where task dynamics should be considered for manipulation, balance, locomotion tasks, etc. Adding to their complexity, humanoid systems are generally underactuated and constrained. Complete representation of the dynamics must consider the 6 DOF floating base link connected passively to an inertial reference frame. Contacts with the environment also input constraint forces, and contact conditions may dynamically change, especially during locomotion. We would like to fully understand how underactuation and constraints interact with the dynamics of the tasks we need to control.
Several contributions have addressed operational space control within constrained environments. Khatib [4] considered constraints applied at the end-effector as an additional task to be controlled, for example to realize a desired contact force. Russakow et al. [9] examined systems with more complex kinematic structures including internal closed kinematic chains. de Sapio and Khatib [2] took a more general approach that included systems with holonomic constraints, and applied it to the operational space control of a parallel mechanism. Sentis et al. [10,11,12] developed operational space controllers to specifically address the underactuation and constrained dynamics of humanoid robots. Further works by Park, Sentis, and Khatib [7,13] additionally consider the control of contact forces for humanoids.
A common theme among the above works is that the controllers are designed to remain dynamically consistent with the constraints (i.e. the controllers contribute no acceleration at the constraint locations). However, because constraints, by definition, are able to apply necessary forces to maintain their consistency, controllers do not need to address the dynamic consistency explicitly. 1 As a consequence, many of the above controllers can be significantly simplified, for example, by using orthogonal projections in place of the oblique projections containing the inertia matrix. For example, Aghili [1] was able to formulate both the direct and inverse dynamics of constrained systems using orthogonal projections derived from only kinematic parameters, which in practice are generally easier to model accurately than inertial parameters [6]. Mistry et al. [5] was also able to apply orthogonal projections for the inverse dynamics control of legged robots, without requir-ing measurement or estimation of contact forces, or oblique projections involving inertia matrices.
One of the objectives of this paper is examine the role of orthogonal projections on constrained operational space dynamics. Working directly with the equations of projected inverse-dynamics [1], we are able to formulate operational space dynamics without consideration of constraint forces or constraint dynamic consistency, leading to a much simplified and manageable solution that retains a similar form to the original unconstrained formulation. Additionally we examine the consequences of passive DOFs and underactuation. Similarly as Aghili [1] used additional constraint forces to influence torque at passive joints, we use dynamically consistent nullspace forces to generate torque at passive joints without affecting desired task dynamics. These null-space forces can be used to either generate constraint forces or null-space motion. We are also able to apply the same control solution to unconstrained systems, using null space motion to compensate for passive degrees of freedom. We attempt to formulate all our results in a unified manner, keeping the structure consistent regardless if the system is constrained or unconstrained, fully actuated or underactuated. We also present examples using a simple but insightful 3 DOF simulated arm.

II. OPERATIONAL SPACE DYNAMICS
We assume the robot with n degrees-of-freedom (DOFs) is represented by the configuration vector q ∈ R n . Additionally, the robot may have k linearly independent constraints such that locations in constraint undergo zero acceleration. We write this condition asẍ C = J Cq +J Cq = 0, where J C is the constraint Jacobian. Note that these constraints can either be either holonomic or nonholonomic [8]. The robot may also be underactuated, in which case we have p active joints and l = n − p passive joints. We write the rigid-body inversedynamics equation as: where M(q) ∈ R n×n is the inertia matrix, h(q,q) ∈ R n is the vector of centripetal, Coriolis, and gravity forces, τ ∈ R n is the vector of joint torques, is the projector into actuated joint space (I p is the p dimensional identity matrix), and λ ∈ R k is the vector of k linearly independent constraint forces We also have an m DOF task, e.g. control of an endeffector, represented by the operational space configuration vector x ∈ R m , and the task Jacobian J, defined by the relationẋ = J(q)q. To summarize the contributions of Khatib [4] for unconstrained (k = 0) and fully actuated (l = 0) systems (e.g. a traditional robot manipulator), the dynamics in the operational space are derived as: where Λ = JM −1 J T −1 and F is an external force applied at the end-effector. Then Khatib formulated the operational space control equation, for redundant manipulators (m < n), as: where F is defined by (3) using a desired task accelerationẍ des in place ofẍ, and J T # is the following generalized inverse of J T : As discussed by Khatib, this generalized inverse is defined to be dynamically consistent with the task: it is the only generalized inverse that results in zero end-effector acceleration for any τ 0 . This inverse also solves the equationẋ = Jq for the joint velocities that minimize the instantaneous kinetic energy of the system. By using (4) we are able to compensate for task space dynamics, such thatẍ =ẍ des , while decoupling motion generated by τ 0 from affecting task-space dynamics.

A. Constraints
Now we extend the operational space formulation to constrained systems using orthogonal projections. Constraints are defined as in (1), and can include systems with closed kinematic chains, such as parallel mechanisms or humanoid/legged robots with two or more feet in contact. At first we address fully actuated systems, and later extend to the underactuated case. As outlined by Aghili [1], we can project (1) into the null-space of the constraints, eliminating the constraint forces from the dynamics equation, and write the equation of projected inverse dynamics: where P is an orthogonal projection operator, such that PJ T C = 0 and P = P 2 = P T . Also, P is readily computable from the constraint Jacobian: P = I − J + C J C (where + indicates the Moore-Penrose pseudoinverse). Note that P depends only on kinematic parameters and unlike other approaches that attempt to maintain dynamic consistency with constraints, does not require the inertia matrix.
Next, we wish to invert PM to solve forq, however because P is generally rank deficient, this term will not be invertible. However, because our system is constrained, we have the following additional equations: where C is (d/(dt)) P, e.g. C = −J + CJ C . Employing a trick used by Aghili, because (8) is orthogonal to (6), we can add these equations: defining M c = PM + I − P. Note, as discussed by Aghili, the choice of M c for a given q is not unique, but is always invertible (provided M is invertible). 2 Next, we multiply (9) by JM −1 c , and replace Jq withẍ −Jq: A force at the end-effector F, is mapped into joint torques via: τ = J T F, and therefore we derive the operational space dynamics as: c P will be unique (for a given q) (see Appendix A), and therefore (11) is also unique.

B. Operational space control with constraints
The operational space control equation for constrained (fully-actuated) systems also takes the form of (4). However, we use F defined by (11) (again replacingẍ with our desired task accelerationẍ des ), and using the following generalized inverse of J T : It is straightforward to verify that this generalized inverse is dynamically consistent: by applying (12) to (4), and then subsequently to (11), we see thatẍ = 0 for any τ 0 . Also because M −1 c P is unique, the dynamically consistent inverse is also unique. Additionally, this generalized inverse minimizes the instantaneous kinetic energy in the constrained space (see Appendix B). For notational simplicity, we define and write the control equation as:

C. Reducing control torques
Equation (13) is a joint space controller that emulates a force F, as if it were externally applied to the end-effector. This controller replicates both the joint motion and constraint forces that would result from the external force. However, constraint forces generate no robot motion, and ultimately no acceleration of the end-effector. Thus we can also consider a control equation that produces the joint acceleration we desire without adding extraneous constraint force. We rewrite the control equation as a sum of three components: where PJ T F generates end-effector acceleration, PNτ 0 generates acceleration only in the null-space (with zero endeffector acceleration), and (I − P) τ C produces only constraint forces and no joint space motion. If we are not concerned about the constraint forces we generate, we can reduce the norm of our torque vector by setting τ C = 0. This controller will produce motion identical to (13), but with different constraint forces and a smaller total control input (since P is an orthogonal projection, we can verify that Pτ ≤ τ , ∀τ ).

III. UNDERACTUATION
When the system contains passive degrees of freedom (l > 0), we may no longer be able to generate desired end-effector forces using the direct mapping to joint torques via a Jacobian transpose. The torques we can generate are limited by the following constraint which must always be satisfied: Unless J T F = BJ T F, we will have to add a null-space component to satisfy (15). However we are still guaranteed that this additional torque will not affect the task-space dynamics. Note, because we cannot compensate for task-space dynamics without the addition of null-space torques in general, we are no longer able to decouple task and null space dynamics. Applying (13) to (15) results in and we can solve for τ 0 : again using the Moore-Penrose pseudoinverse. Provided (17) has at least one valid solution for τ 0 , we can use (18) in (13) and write the control equation as: Also, because (I − B) is an orthogonal projection, the equation simplifies to (see Appendix C): If the system has sufficient redundancy, there may be an infinite number of solutions for (17). However, by using the Moore-Penrose pseudoinverse, we are computing the minimum possible τ 0 , and (20) represents the operational space control solution with the minimum possible null space affect. Note that (20) can work for both unconstrained and constrained systems (using either (3),(5) or (11),(12), respectively), again provided a solution to (17) exists. In the case of unconstrained systems, the controller will generates dynamically consistent null-space motion to compensate for lost torque at the passive joints. However, in the constrained case, the controller uses a combination of both motion and constraint forces to generate torque at the passive joints.

A. Multiple tasks
If we have more than one task, we can address underactuation using dynamically consistent torque as above, but the torque must be dynamically consistent with all tasks. As an example, consider two tasks with task 1 having a higher priority than task 2. We define the augmented Jacobian of all tasks as J T a = J T 1 J T

2
, and the null space projector that is dynamically consistent with all tasks as: N a = I − J T a J T # a . Then we write the operational space control equation as: Solving for τ 0 as done previously, we can write the control solution as:

B. Joint acceleration consistant controller
The multi-task solution above assumes we have sufficient redundancy remaining after the assignment of all tasks. If all DOFs of the robot are accounted for in J a (e.g. Rank(J a ) = n), then it is impossible to add any motion that will not conflict with at least one task. However, if the robot has constraints, we may still be able to add constraint forces to resolve underactuation, without inducing any additional joint space motion at all. We use the equation: and even further reduces to: This solution is significant for a number of reasons. First, it produces identical joint accelerations as the operational space controller of a fully actuated (but constrained) system. Therefore the null space component, required to resolve underactuation, adds no additional kinetic energy into the system. Secondly, the projector [PB] + is identical to the projector of constrained underactuated inverse dynamics controllers. For example, to achieve a desired joint space accelerationq des , providedq des is constraint consistent, we can use the following controller independent of constraint forces [5]: Finally, (26) is identical to the operational space controller derived by Sentis et al. for humanoid and legged robots, but in a much reduced form. For example, the inertia matrix is only used in F and does not appear in the projector or Jacobian as in [12]. Note that this point is more throughly developed in [3], and here we additionally verify it empirically. FA is the fully actuated controller. Note that controllers FAr and UACF have identical joint motion as FA, and therefore identical task performance. UAN has similar performance to FA, however UAPN has increased oscillation, likely a consequence of not using constraint forces.  Fig. 3. Instantaneous kinetic energy of the system (39). FA is the fully actuated controller, and FAr and UACF have identical kinetic energy profiles. Since UAN adds additional null space motion to resolve underactuation, it has a slightly different energy profile. UAPN does not use constraint forces, and therefore requires significantly larger energy to resolve underactuation.

C. Motion only controller
Since the previous controller (26) only uses constraint forces to resolve underactuation, for comparison, we can consider a controller that only uses null space motion. The controller is written as: and attempts not to change the constraint forces generated task dynamics.

IV. EXAMPLES
In order to test the validity of our controllers, as well our understanding of constrained operational space dynamics, we aim to use the simplest possible system that still makes sense . UAPN only uses motion to resolve underactuation, and therefore has a force profile closest to FA. UACF uses only constraint forces to resolve underactuation, and therefore has identical motion to FA, but with significantly larger constraint forces. The UAN controller, which minimizes the null-space torque contribution, strikes a balance between UACF and UAPN.
to add constraints and/or underactuation. We use a simulated 3 DOF planar arm with rotary joints. Each link of the robot simulator has the same length and the same inertial parameters. We assume zero gravity and include a small amount of viscous friction at the joints. The system also has a 1 DOF constraint: the x position of the end-effector, and a 1 DOF task: the y position of the end-effector. Fig. 1 (upper left box) describes the setup and initial condition. To consider underactuation, we may turn off the actuator at joint 3 (green circle in Fig. 1).
The task is to regulate to a new end-effector position x target , and we set our desired task acceleration as: We test the 5 constrained operational space controllers labeled as follows.
FA : The fully actuated operational space controller of (13), with τ 0 = 0 FAr : The fully actuated controller with reduced torque (14), with τ 0 = 0, τ C = 0 UAN : The underactuated controller that minimizes the null space contribution (20) UACF : The underactuated controller that uses only constraint forces (and therefore minimizes instantaneous kinetic energy) (26) UAPN : The underactuated controller that uses only null space motion. (28) We use the same control parameters (gains) for each case. Fig. 1 shows the resulting motion and constraint forces after simulating for 2 seconds. Fig. 2 also shows the task regulation performance. As expected, the fully actuated controllers FA and FAr have identical joint motion, but very different constraint forces. We computed the sum of the norm torque over 2 seconds and found that indeed, FAr reduced the sum of the norm torque by 30.0%. As a consequence, contact forces are increased. In the underactuated case, all three controllers achieved that task in spite of a passive joint. UACF produced identical motion to the fully actuated controllers, however had a noticeable increase in constraint force profile. UAPN, which uses purely null space motion to resolve underactuation, significantly increases the kinetic energy of the system (Fig. 3), however, it maintains the most similar constraint force profile to FA of the three underactuated controllers. By minimizing  Fig. 4. Comparison of fully actuated (left) and underactuated (right) multitask controllers. The primary task is reaching the y-axis target, and secondary task is the x-axis target (dashed lines). Both controllers are able to achieve the task but have very different joint motion. Primary task performance of the unconstrained system. The underactuated robot is in blue. null space impact, UAN seems to reach a compromise between the two: a slight increase in kinetic energy in trade for a relatively lower contact force profile.
A nice property of some of the equations we derived, is that they retain the same form whether the system is constrained or unconstrained. Thus we may be able to use them to control operational space dynamics for systems with passive joints and no constraint forces. To test, we remove the constraint from the 3 DOF arm, and add a secondary task to control endeffector position in the x direction. Then, we validate with two controllers: FAuc : A fully actuated, multi-task controller using (21) with τ = 0 UANuc: An underactuated multi-task controller using (22). Joint 3 is still considered as the passive joint. The resulting simulation is shown in Fig. 4 (the underacted robot is on the right), with primary and secondary task performance shown in Figs. 5 and 6. We also plot the instantaneous kinetic energy (Fig. 7) which confirms that the underactuated robot will use more energy by generating null space motion to compensate for torque at the passive joints. Note that this controller is similar to partial feedback linearization  Fig. 7. Instantaneous kinetic energy of the unconstrained experiment. Not surprisingly, the underactuated robot requires more motion to compensate for the passive joints. [14], however rather than linearizing the dynamics of active or passive joints, we are linearizing the operational space dynamics.

V. CONCLUSION
In summary, we have presented a new manner for considering operational space control of constrained and underactuated systems. First, for constrained systems, we are able to take advantage of orthogonal projections, which significantly simplifies the formulation and derives operational space dynamics without a need to explicitly consider constraint forces or constraint consistency. Next we are able to address the control of underactuated systems, but now have a better comprehension of the underlying solution: additional null space forces are required to compensate for passive DOFs. Thus, as a consequence, we can no longer decouple task and null space dynamics. However, if our system has sufficient redundancy, we have some choice of how to approach the problem: we can use constraint forces, which do not add more kinetic energy into the system, or we can add movement within the null space, which may create more desirable constraint forces. Thus there exists a trade-off between motion and constraint forces that can potentially be exploited. For example, there may be configurations where one solution is singular, but another is not. Fig. 8 shows such a configuration of the 3 DOF arm where (26) fails: the constraint force is directly aligned with the passive joint and therefore cannot contribute torque to it. However, this configuration presents no problems for the other controllers that contribute torque by moving. Finally, in this work we have addressed the optimality of some of the derived controllers (for example, (20) minimizes the instantaneous null space contribution). However, dynamic optimization tools may be required to optimize over complete trajectories, for example to plan the avoidance of singularities.
and take the singular value decomposition (SVD) of M c = UΣU T . Because PMP and (I − P) M (I − P) are both symmetric and mutually orthogonal, we can separate the singular values as: with PMP = U 1 Σ 1 U T 1 and (I − P) M (I − P) = U 2 Σ 2 U T 2 . Similarly for the inverse: Finally, because P spans the null-space of (I − P) M (I − P), and likewise the null-space of ((I − P) M (I − P)) + , we can write: We can repeat a similar procedure for the other choices of M c , showing that (37) always holds. Thus, (37) is independent of choice of M c and is unique.

B. Minimization of Kinetic Energy
We prove that the following inverse kinematics solution: computes joint velocities that minimize the instantaneous kinetic energy of constrained motion: Using the result (37) and same SVD from Appendix A: we see the RHS minimizes the quantity: (44) and therefore minimizes (39).

C. Simplifications using orthogonal projections
We call R (A) the range of the matrix A. From the SVD of A = UΣV T , and its pseudoinverse A + = VΣ + U T , we know that R (A) = R A + T and R (A + ) = R A T . Take two projection operators X and Y and the expression: