Inverse Dynamics Control of Floating-Base Robots ... - Semantic Scholar
Inverse Dynamics Control of Floating-Base Robots with External Constraints: a Unified View Ludovic Righetti, Jonas Buchli, Michael Mistry and Stefan Schaal Abstract— Inverse dynamics controllers and operational space controllers have proved to be very efficient for compliant control of fully actuated robots such as fixed base manipulators. However legged robots such as humanoids are inherently different as they are underactuated and subject to switching external contact constraints. Recently several methods have been proposed to create inverse dynamics controllers and operational space controllers for these robots. In an attempt to compare these different approaches, we develop a general framework for inverse dynamics control and show that these methods lead to very similar controllers. We are then able to greatly simplify recent whole-body controllers based on operational space approaches using kinematic projections, bringing them closer to efficient practical implementations. We also generalize these controllers such that they can be optimal under an arbitrary quadratic cost in the commands.
I. INTRODUCTION One of the long standing goals of robotics research is to develop robots that can autonomously, dexterously and safely interact outside of carefully designed artificial environments. For instance, they would be able to operate in places where dextrous and safe interaction with humans is required, or in highly complex terrains where agility and robust performance in light of many disturbances is in the foreground (e.g. disaster sites). The development of such robots needs control algorithms that ensure high tracking performance and compliance at the same time in order to provide agile motion and dextrous manipulation while guaranteeing compliant contact interaction and safe interaction with humans. Traditional position control schemes are not suitable for such purposes since they inherently require high gains and therefore high stiffness for good tracking performance. In contrast, torque control strategies have recently been successfully implemented for the control of complex, redundant robots [1]–[3]. Especially model based approaches such as inverse dynamics controllers or operational space controllers are very appealing, as, they allow to significantly lower the gains and therefore allow for more compliant control while also guaranteeing high tracking performance. The general idea behind these model-based control scheme is to use a dynamic model of the robot in order to compute the necessary torques to achieve a desired movement. In addition to this feedforward command an error feedback command (e.g. a PID controller) with low gains is added This research was supported in part by National Science Foundation grants ECS-0326095, IIS-0535282, IIS-1017134, CNS-0619937, IIS0917318, CBET-0922784, EECS-0926052, the DARPA program on Advanced Robotic Manipulation, the Army Research Office, the Okawa Foundation, the ATR Computational Neuroscience Laboratories and the Swiss National Science Foundation. L. Righetti and S. Schaal, Computational Learning and Motor Control Lab, University of Southern California Los Angeles, CA90089, USA [email protected], [email protected] Jonas Buchli, Dept. of Advanced Robotics, Italian Institute of Technology, Genoa, Italy [email protected] Michael Mistry, Disney Research Pittsburgh, Pittsburgh, PA15213, USA [email protected]
to handle movement errors coming either from model uncertainty, parameter estimation errors, or external disturbances. Traditionally we can distinguish two control approaches for fully actuated unconstrained robots (typical examples are manipulators fixed on the ground). The first approach is to directly design joint trajectories and use an inverse dynamics controller to compute the necessary torques to achieve the desired motion and a PID controller with low gains in joint space for disturbance rejection. The second approach is to design task-space trajectories (e.g. the endeffectors) and use an operational space controller to compute the torques necessary to achieve the movement in task space. The error feedback is also designed in task-space coordinates. Additionally a command is generally added to optimize a desired posture in the nullspace of the task. There are several different approaches to design operational space controllers. One of the best known approach is the one introduced by Khatib [4] which has many interesting properties such as decoupling between task and nullspace forces and accelerations. An overview of these approaches together with experimental comparisons were presented in [5]. Recently [6] proposed a unified view where operational space control is formulated as a special class of optimal tracking controllers. Each approach to operational space control corresponds to a tracking controller that is instantaneously optimal with respect to a particular quadratic cost in the commands. However legged robots are inherently different from manipulators since they are underactuated systems due to their floating-base1 . Moreover these robots are subjected to switching contact constraints. Therefore legged robots cannot be directly controlled with the well-known control schemes described above. From the control point of view, we can distinguish three cases: • Legged robots can be seen as fully actuated when there are as many constraints as free floating DOFs (e.g. a humanoid is standing on one foot). The dynamic model can be reduced to a fixed-base manipulator model. • Legged robots can be underactuated when there are less constraints than free floating DOFs (e.g. a humanoid jumping in the air). • Legged robots can be overactuated when there are more constraints than free floating DOFs (e.g. a humanoid with two feet on the ground) and there exists an infinite choice of possible torques to achieve the desired constraint-consistent motion. It is possible to control such robots using different models and controllers for each constraint situation. However this approach is not practical because it requires as many models and controllers as constraint situations. Moreover each model 1 In order to generally describe the dynamics of a legged robot one has to add to the n degrees of freedom of the joints 6 degrees of freedom corresponding to the position and orientation of the robot relative to an inertial frame.
is derived in a constraint consistent subspace without explicitly computing these constraints. Therefore they are often not well suited for the control of contact constraints. It is also desirable to avoid measuring constraint forces since it would require sensors at each contact point and can lead to brittle control due to noise measurement and delay due to filtering. The ideal situation would be to have a single controller for all these different cases that can handle switching external contact constraints and that does not require constraint forces measurement. Recently such an approach has been taken for both operational space and joint position control of legged robots. Sentis et al. [1], [7] proposed an extension of operational space controllers for floating-base robots and for prioritization of tasks that requires only one dynamic model of the robot and that can handle various types of external constraints. They design a hierarchy of constraint-consistent tasks (e.g. movement of the center of mass and movement of the hands), each task acting in the nullspace of both higher priority tasks and the constraints. This approach is very appealing since it virtually allows one to control any desired position or force in task space and to superpose these tasks together for floatingbase robots under constraints. However one caveat of this approach is the complexity of the controller. Indeed among all of the needed computations, it requires the inversion of the rigid body inertia matrix in several places making this approach sensitive to modelling and parameter estimation errors [5]. To the best of our knowledge this approach was never applied on a real robotic platform. Another approach for joint space trajectory control was recently proposed by Aghili in [8], based on orthogonal projections. The inverse dynamics is computed by projecting the dynamics into a constraint consistent manifold thus removing the constraints. The advantage of such an approach is that it can also handle constraint switching and is computationally efficient since it is based on the decomposition of the constraint Jacobian, which is a purely kinematic quantity. More recently, based on similar ideas, Mistry et al. [3] proposed an inverse dynamics algorithm for legged robots with switching contact constraints. This algorithm is very appealing since it does not require the inversion of the inertia matrix and requires only kinematic terms and an estimation of the unconstrained inverse dynamics, which is computationally inexpensive. In addition it allows to predict the constraint forces efficiently using only kinematic projections, which can be used to develop force controllers [9]. This controller was also successfully applied on real robotic platforms demonstrating its robustness and efficiency for real-time control [3], [9]. The controllers mentioned above were independently derived for very different purposes and the control laws have very different forms. Several questions arise. To what extend are these control laws different? Is it possible to benefit of the advantages of the hierarchical task controllers while preserving the simplicity and robustness of the inverse dynamics controller based on orthogonal projections? Is it mandatory to use the inverse of the inertia matrix for the operational space approach? In this paper, we demonstrate the surprising insight that these previous different approaches to floating base inverse dynamics control can indeed be thought as equivalent. In order to do so, we develop a general framework to describe such controllers and prove the equivalence of all these controllers. This general framework for inverse dynamics
control of under-actuated robots under constraints is the main contribution of this paper. The consequences of this result are two-fold. First, we give a strong argument in favor of the use of orthogonal projections for inverting the dynamics [3], [8] since these approaches are equivalent to the ones using dynamic quantities and are easier to implement on real robotic systems. Second, we are able to simplify the controller presented in [1], [7] to work with kinematic projections and extend it to be optimal with respect to any quadratic cost in the commands. In Section 2 we present the general framework that will allow us to compare the different approaches and derive the main result of the paper. In Section 3 we discuss the practical consequences of this result, present the controllers that we wish to compare and simplify the operational space controller. We then show numerical simulations to support the mathematical results in Section 4 and discuss the computational aspects of the new results and their applicability on real platforms. We then conclude the paper. II. GENERAL FRAMEWORK A. Problem formulation The dynamics of a free-floating rigid-body robot subject to external constraints is generally given by2 M¨ q + h = ST τ + JTc λ
(1)
under the k constraints ¨ = b(q, q) ˙ Jc q
(2)
(n+6)×(n+6)
where M ∈ R is the rigid body dynamics (RBD) Inertia matrix, h ∈ Rn+6 is a generalized force vector containing the Coriolis, centrifugal and gravitational effects, τ ∈ Rn is the actuation vector and S ∈ Rn×(n+6) is the joint selection matrix that reflects the underactuation – for instance, for most floating base robots, S would be the identity matrix in the first n × n submatrix, and zeros elsewhere. Jc ∈ Rk×(n+6) is the Jacobian of the k constraints with the λ ∈ Rk Lagrange multipliers that correspond to the constraint forces. Following the ideas from [10] we express the constraints in acceleration form (Eq. 2) . Holonomic constraints can be expressed by differentiating them twice and non-holonomic constraints by differentiating them once. Without loss of generality, we assume that Jc is full row rank, in the sense that all constraints are linearly independent. Indeed it is always possible to find a reduced number of independent constraints, for example by using the SVD decomposition of Jc . Therefore our results will hold for an arbitrary number of constraints. For example, assume that the position of the point feet of a legged robot are given by xc , then the constraints that the feet do not move relative to the ground can be written as xc = const or equivalently by x˙ c = 0. Relating this to the motion of the joints of the robot using the Jacobian of xc we have x˙ c = Jc q˙ = 0, which we differentiate once again ¨ = −J˙ c q. ˙ to get Jc q We assume that the movement plan of the robot is expressed by desired joint accelerations that are constraint 2 We discuss floating base systems with n DOFS and n actuators, but all the derived results here can be directly applied to systems with a higher number of actuators (e.g. muscle-like systems) and to systems with a higher degree of under-actuation (e.g. a robot with passive elements). These cases are not included to keep notation and discussion as simple as possible.
¨ = q ¨ d holds. These consistent, i.e., Equation (2) with q accelerations will be satisfied if and only if they are of the form G ¨ d = JG q q0 (3) c b + (I − Jc Jc )¨ where JG c can be any generalized inverse [11] of Jc , i.e. a ¨ 0 is an arbitrary acceleramatrix such that Jc JG c Jc = Jc . q tion vector and (I − JG J ) projects these accelerations into c c the null space of the constraints. The general problem of inverse dynamics is to compute the torques τ such that they will achieve the desired accel¨ d . This is equivalent to solving Equations (1) and erations q ¨=q ¨ d . The problem is not straightforward (2) for τ with q since the system is underactuated due to the floating base and its dynamics must satisfy constraints. B. Class of possible controllers The solutions of the inverse dynamics problem will essentially live on a manifold embedded in Rn+6 where the constraints are enforced (i.e. when restricted to this manifold the dynamics can be written without Lagrange multipliers). Lets define a linear operator P(q) of rank n + 6 − k that maps the dynamics onto this constraint consistent manifold such that the dynamics (i.e. the free dynamics) is written as P(M¨ qd + h) = PST τ
in the commands at each instant of time. First we state the following lemma (also discussed in [8]) ¨ d are achievable, Lemma 1: If the desired accelerations q all possible controllers for the inverse dynamics problem under constraints that achieve perfect tracking of these desired accelerations can be written in the form of τ (W, τ 0 )
W
+ (I − (PST ) PST )W−1 τ 0 with
W
(PST )
(5)
¨ , we can and by noting that the constraints enforce b = Jc q insert the constraints into Eq. (1) and we get an equation that it is equivalent to choosing the linear operator Pdirect = (I − JTc (Jc M−1 JTc )−1 Jc M−1 )
(6)
to describe the unconstrained dynamics. Therefore there always exists one such operator. Solving the original problem is then reduced to solving Equation (4). We can already see that the number of solutions to this equation will depend on the number of constraints. Indeed the unconstrained dynamics lies in a n + 6 − k dimensional manifold while the dimension of the control vector is n. There are then 3 distinct cases: • k < 6, the system is underactuated. There is at most one solution to the inverse dynamics problem: for a solution to exist, the desired accelerations must not only be constraint consistent, they moreover need to be consistent with the dynamics of Eq. (1). For example one can think of the case without constraints: a falling cat cannot orient its body and move its joints independently. • k = 6, the system is fully actuated. There is exactly one solution provided that the desired accelerations are constraint consistent. As we already discussed in the introduction this case is similar to the inverse dynamics problem of a manipulator fixed to the ground. • k > 6, the system is overconstrained. There is an infinite ¨ d . It is number of solutions for τ that will achieve q the case, for example, when a humanoid has its two feet on the ground (12 constraints) or when a point feet quadruped robot has 3 feet on the ground (9 constraints). In the following we show that in the overconstrained case we can create controllers that are optimal for a quadratic cost
1
(8)
where () denotes the Moore-Penrose generalized inverse [11] , W is symmetric positive definite3 and τ 0 is an arbitrary torque vector. Moreover the unique controller that minimizes the cost τ T Wτ can be written as W
τ (W, 0) = (PST ) P(M¨ qd + h)
(9)
Proof: The inverse dynamics problem with constraints is equivalent to solving Equation (4). Since W is symmetric 1 positive definite, its square root W 2 exists and is also symmetric positive definite and therefore invertible. We can then write Equation (4) as 1
1
PST W− 2 W 2 τ = P(M¨ qd + h) and then we get the general solution to this equation as 1
λ = (Jc M−1 JTc )−1 (b + Jc M−1 (h − ST τ ))
1
= W− 2 (PST W− 2 )+
(7)
+
(4)
For example if we compute the Lagrange multipliers that are uniquely given by [10]
W
= (PST ) P(M¨ qd + h)
W2τ
=
1
(PST W− 2 )+ P(M¨ qd + h) 1
1
τ0 + (I − (PST W− 2 )+ PST W− 2 )˜ where τ˜ 0 is an arbitrary torque. Therefore we can write it 1 as τ˜ 0 = W− 2 τ 0 and the first part of the lemma follows from standard computations. Since the unique solution to an equation of the form Px = b that minimizes kxk is 1 1 x = P+ b, then τ (W, 0) minimizes kW 2 τ k. Since W 2 is 1 symmetric kW 2 τ k2 = τ T Wτ . This proves the second part of the lemma. Thus given a formulation of constraint consistent dynamics (i.e. the general dynamics restricted to a feasible manifold), we have generated a class of controllers parametrized by W and τ 0 . This result has to be compared to the work of [6] where the authors derive the class of operational space controllers that optimize a similar cost function for fully actuated systems. The main difference from our result is that we characterize underactuated systems with constraints for joint space tracking. Remark Multiple solutions for inverse dynamics control only exist in the overconstrained case, i.e. when there are more constraints than degrees of underactuation. Therefore the parameters W and τ 0 are associated with redundancy resolution at the torque level and will have an effect only when k > 6. In the case k ≤ 6 there is at most one solution to the problem and all parametrizations will lead to the same controller. W
Remark Note that (PST ) is a generalized inverse of W PST with a weight W. Indeed PST (PST ) PST = PST . Moreover using the properties of Moore-Penrose inverses we 1 3W 2
is the square root of the positive definite matrix W, i.e. the unique 1 1 positive definite matrix such that W 2 W 2 = W
can always rewrite this generalized inverse such that it does not require to compute the square root of a matrix W
(PST )
= W−1 SPT (PST W−1 SPT )+
(10)
torque control law for tracking desired joint trajectories qd is then given by τ
1
P1 ST τ 1 P2 ST τ 2
= =
P1 (M¨ qd + h) P2 (M¨ qd + h)
(11) (12)
with their parametrized set of possible controllers τ 1 (W, τ 0 ) and τ 2 (W, τ 0 ). The following equality holds τ 1 (W, τ 0 ) = τ 2 (W, τ 0 ) (13) Remark This results shows that the controllers can differ only in the way the torque redundancy resolution is done (i.e. the choice of W and τ 0 ). The choice of Pi does not create new controllers and is only important from a computational point of view. This constitutes a strong argument for the use of the simplest projection possible that uses only kinematic parameters to compute the controller. III. PRACTICAL CONSEQUENCES We now discuss the consequences of the previous results for the controllers we described in the Introduction. A. Constraint elimination via orthogonal projections Mistry et al. [3] recently proposed an inverse dynamics control law based on the orthogonal decomposition of the constraint Jacobian. It was originally designed to handle fully or overconstrained cases and holonomic constraints. Here we rewrite their controller and note that this controller can in fact handle any types of constraints written in the form of ¨ d is Eq. (2) and works also in the underactuated case (if q achievable), since the Moore-Penrose inverse always exist. Assume the QR decomposition of the constraint Jacobian as JTc = Q[RT 0]T . We can multiply Equation (1) by QT and get the following decoupled dynamics Su QT ST τ λ
= Su QT (M¨ qd + h) (14) = R−1 Sc QT (M¨ qd + h − ST τ ) (15)
where Su = [0(n+6−k)×k I(n+6−k)×(n+6−k) ] selects the columns of QT corresponding to the unconstrained space and Sc = [I(n+6−k)×k 0(n+6−k)×(n+6−k) ] selects the constrained dimensions. The inverse dynamics controller follows if we take PQR = Su QT (16) In this case the linear operator PQR is a purely kinematic operator since it uses only the constrained Jacobian. This formulation is very simple and practical since it merely requires a kinematic projection and estimation of the unconstrained inverse dynamics. Moreover it gives an equation for the constraint forces that is very simple when compared to Eq. (5) and can be used to develop force controllers [9]. The
1
1
+ (I − W− 2 (PQR ST W− 2 )+ PQR ST )W−1 τ 0 + PID (qd , q˙ d ) (17)
C. Invariance of the controllers under projection choice We use the previous formulation to state the following lemma, which is one of the main contributions of this paper (the proof is given in the appendix) Lemma 2: Given two different linear operators P1 and P2 that describe the same system with constraints
1
= W− 2 (PQR ST W− 2 )+ PQR (M¨ qd + h)
B. Operational space control The operational space formulation derived by Khatib [4] was recently extended to include task hierarchies on floating base systems by Sentis et al. [1], [7] and to control the center of pressure of contacts using the nullspace torques (using τ 0 in our framework) [12]. The controller that is presented in [7], [12] is written as τ
=
(SNc ST )+ SNc ! N X T T × (Js|prec(s) Fs ) + Nt τ posture s=1
+ (I − (SNc ST )+ (SNc ST ))τ 0
(18)
with Nc = M−1 I − JTc (Jc M−1 JTc )−1 Jc M−1
(19)
where JTs|prec(s) are prioritized Jacobians of prioritized operational space tasks, Fs are dynamically consistent control forces, NTt is the cumulative prioritized null space matrix associated with higher priority tasks and τ posture is a postural control vector. If we write ! N X ¨ d = M−1 q (JTs|prec(s) Fs ) + NTt τ posture − h (20) s=1
then this controller can be thought as an inverse dynamics controller and therefore be cast in our framework by selecting Pop.sp. = SNc
(21)
where W = I. We note that in the absence of internal torques τ 0 = 0, the original controller used in [7] minimizes the cost τ T τ , since W = I. Using the results presented here we can generalize this controller to minimize any quadratic cost in the commands by selecting a different W. Second we note that Nc ∈ R(n+6)×(n+6) is a relatively complex terms where there are 3 occurrences of the inverse of the inertia matrix. In the overconstrained case SNc ST will be rank deficient, requiring special numerical methods to compute the MoorePenrose inverse while it will not be the case for PQR . Since we have shown that different linear operators lead to the same controllers in Lemma 2, we can replace Pop.sp. by PQR and get a simpler but equivalent whole body controller τ
1
1
= W− 2 (PQR ST W− 2 )+ PQR ! N X T T × (Js|prec(s) Fs ) + Nt τ posture s=1
+ (I − W
− 21
1
(PQR ST W− 2 )+ PQR ST )W−1 τ 0 (22)
where we removed the unnecessary appearances of the inertia matrix. The reformulation of the controller is interesting as it greatly simplifies the computation of the controller using only kinematic parameters for the projection. Moreover this
controller generally minimize any cost τ T Wτ when we choose τ 0 = 0 and as such represents a generalization and simplification of the previous approach. IV. EXPERIMENTS In the following we present numerical evaluations to confirm the results presented in the previous sections. We also discuss the computational aspects of the various controllers and therefore their applicability on real robotic platforms. A. Description of the controllers In the following experiments we use the three linear operators discussed in this paper. First the operator that arises when computing the Lagrange multipliers as we showed in Eq. (6) Pdirect = (I − JTc (Jc M−1 JTc )−1 Jc M−1 )
(23)
Second, the operator resulting from the QR decomposition of the constraint Jacobian PQR = Su QT
(24)
Finally, the operator derived from the whole-body controllers using the operational space formalism Pop.sp. = SM−1 I − JTc (Jc M−1 JTc )−1 Jc M−1 (25) In order to show the generalization of the controllers to minimize any quadratic cost in the commands, we define three different costs. 1 We minimize total torque τ T τ by selecting W = I 2 We into account actuator saturation and minimize P take τi 2 i ( τ i,max ) where τ i,max is the maximum torque al1 lowed for actuator i. In this case W = diag{ (τ i,max )2 } 3 We use the inertia matrix as metric, which is a popular metric for manipulators [4], [6] and choose W = SM−1 ST , which corresponds to the inverse of the joint space inertia matrix. Note that in every case we use τ 0 = 0. The desired accelerations are computed using an operational space controller for the position of the base of the robot (i.e. its trunk) together with a control of a default posture for the joints in the nullspace. The full state constraint consistent desired ¨ d are computed as accelerations q ¨d q
¨ posture = −Jc q˙ + (Jb Nc )+ x ¨d + (SNb )+ q
x ¨d
=
q ¨posture
Kbp (xd − x) + Kbd (x˙ d − x) ˙ −
d(Jb Nc ) x˙ dt
= Kjp (qposture − qj ) − Kjd q˙ j −
d(SNb ) q˙ j dt
where Nc = I − J+ c Jc is the nullspace projector of the constraint Jacobian, Jb is the Jacobian of the base position and Nb = I − (Jb Nc )+ Jb Nc is the nullspace projector of the constraint consistent Jacobian Jb Nc . The actual and desired position of the base and the joints current and default positions are denoted by x, xd , qj and qpost respectively. The matrices Ki are positive definite gain matrices. We perform the tests on a physics simulation of a biped humanoid that is modeled according to the Sarcos Humanoid, which is a 34 DOFs human-sized humanoid robot (Fig. 1).
Fig. 1.
Picture of the robot used for the simulations
B. Results and computational considerations The controllers perform a sine movement on the lateral coordinate of the base (i.e. moving its weight from left to right) and a sine movement in the vertical coordinate (i.e. moving up and down) while keeping the position of the sagittal coordinate constant. Figure 2 shows the result of the simulations. As expected we see that the controllers differ only from the redundancy resolution schemes, W, and that the choice of the linear operator P does not change anything. We note that the redundancy resolution leads to very different torques distributions and as expected the minimum costs are attained when choosing the appropriate weights. From a practical point of view, the choice of the linear operator will be critical in order to create controllers that can be run in real-time and that are numerically stable. We have shown that the use of the inertia matrix was not important and constituted an additional complexity for numerical computation4 . On the other hand computing the QR decomposition of the constraint Jacobian is extremely fast on modern computers by using a SVD decomposition and can be used in real-time control loops. From these considerations, this results constitute a strong argument for the use of kinematic projections for inverse dynamics controllers and operational space controllers. The new formulation of the whole-body controller of [1], [7] constitutes a real improvement from the computational point of view for practical implementations. V. CONCLUSION In an attempt to compare different control approaches for inverse dynamics and operational space control for floating base robots, we developed a general framework for inverse dynamics control of underactuated systems under constraints. We derived the class of possible controllers for inverse dynamics together with optimality results for any quadratic cost in the commands. We then showed that recent approaches for operational space control [1], [7], [12] and inverse dynamics [3] were in fact equivalent. From these results we were able to show that the wholebody controller proposed by [1] could be thought as an inverse dynamics controller. We were then able to greatly simplify it using kinematic quantities. We also generalized this controller such that we can optimize any quadratic cost in the commands. Since the linear operators used for the inverse dynamics is theoretically not important, it constitutes a strong argument for using operators based on kinematic quantities as they are simpler to use and already proved to be robust when applied to real robotic systems [3], [9]. It also shows that the use of the inverse of the inertia 4 Note that our result also shows that theoretically the use of a badly estimated inertia matrix does not change anything. However this could be problematic from a numerical point of view.
2
4
6
8
10
Base y
0 ï0.02 ï0.04 0
2
4
6
8
10
Base z
0 ï0.05 ï0.1 0
2
4
Time [s]
6
8
10
2000 1000 0
2
4
6
8
10
4
x 10 8 6 4 2 0
15 10 5 0 ï5 0
2
4
6
8
10
3 2 1 0 0
Left Hip Flex/Ext Torque [N.m]
ï0.05 0
20
3000
2
4
Time [s]
6
8
10
Left Ankle Ad/Abd Torque [N.m]
Total Torque Cost
0
Torque Inertia Weighted Saturation Cost Cost
Base x
0.05
2
10
4 6 control 8 Total Torque Inertia weighted control Actuator Saturation control
10
5 0 ï5
ï10 0
2
4
Time [s]
6
8
10
Fig. 2. Result of the experiments described in Section IV. The left figure shows the movement of the base for the different controllers. The middle figure shows the instantaneous value of the different costs we optimize and the right one shows the torques applied. The results from all 9 controllers are superposed (3 projections Pi tested with 3 redundancy resolution schemes). The redundancy resolution corresponding to optimization of τ T τ is in black dashed/dotted line, the one corresponding to τ T SM−1 ST τ is in dashed blue line and the one corresponding to minimization of actuator saturation is in plain red line. All the three controllers derived from different projections Pi are superposed. The left figure shows that all the controllers produce the same task space motion. The middle and right figures show that the choice of W leads to different commands while the choice of P does not change anything since the commands are superposed and cannot be distinguished.
matrix does not provide any theoretical advantages for these types of control. From a larger perspective, these results are interesting since they provide a unified framework for comparing very different controllers and to derive optimality results. A PPENDIX P ROOF OF L EMMA 2 The two controllers τ 1 (W, 0) and τ 2 (W, 0) must be equal since they both are controllers that achieve perfect tracking of the desired accelerations and minimize the cost τ T Wτ while there exist only one controller that minimizes this cost because of Lemma 1. Therefore we have τ 1 (W, 0) = τ 2 (W, 0)
(26)
which implies (P1 ST W
− 12
)+ P1 (M¨ qd +h) = (P2 ST W
− 12
)+ P2 (M¨ qd +h) (27) Since both Equations (11) and (12) describe the same constraint system, their sets of possible controllers are the same. τ 1 is then a solution to P2 ST τ 2 = P2 (M¨ qd + h) and we can write T P2 (M¨ qd + h) = P2 S τ 1 1
1
we multiply by P2 ST W− 2 (P2 ST W− 2 )+ to get 1
1
P2 ST W− 2 (P2 ST W− 2 )+ P2 ST τ 1 1
1
qd + h) = P2 ST W− 2 (P2 ST W− 2 )+ P2 (M¨ = P2 ST W
− 12
1
(P1 ST W− 2 )+ P1 (M¨ qd + h)
where the last equality is found by using Eq. (27). Developping τ 1 using Eq. (7) and simplifying we finally get 1 1 1 1 P2 ST W− 2 I − (P1 ST W− 2 )+ P1 ST W− 2 W− 2 τ 0 = 0 (28) This equation means that any vector that is in the nullspace 1 1 of P1 ST W− 2 is also in the nullspace of P2 ST W− 2 . Conversely we can derive a similar equation that will show 1 that any vector in the nullspace of P2 ST W− 2 is also in T − 12 the nullspace of P1 S W . These nullspaces are then the same. Now we look at the difference of the controllers τ 1 and τ 2 . After simplification we get 1 1 1 τ1 − τ2 = W− 2 (P1 ST W− 2 )+ P1 ST W− 2 1 1 1 −(P2 ST W− 2 )+ P2 ST W− 2 W− 2 τ 0
Inserting Equation (28) we can then find 1 1 1 τ 1 − τ 2 = W− 2 (P2 ST W− 2 )+ P2 ST W− 2 − I 1 1 1 × (P1 ST W− 2 )+ P1 ST W− 2 W− 2 τ 0 (29) 1 1 Since (P1 ST W− 2 )+ P1 ST W− 2 projects in the orthogonal 1 complement of the null space of P1 ST W− 2 then it also projects in the orthogonal complement of the null space of 1 P2 ST W− 2 and as a consequence the right hand side of Eq. (29) must be zero. This proves that for a given set of W and τ 0 , the two controllers τ 1 and τ 2 are the same and this completes the proof. R EFERENCES [1] L. Sentis and O. Khatib, “Synthesis of whole-body behaviors through hierarchical control of behavioral primitives,” International Journal of Humanoid Robotics, vol. 2, no. 4, pp. 505–518, 2005. [2] S. Hyon, J. G. Hale, and G. Cheng, “Full-body compliant humanhumanoid interaction: Balancing in the presence of unknown external forces,” IEEE Trans. on Robotics, vol. 23, no. 5, pp. 884–898, 2007. [3] M. Mistry, J. Buchli, and S. Schaal, “Inverse dynamics control of floating base systems using orthogonal decomposition,” in In proc. of the 2010 International Conference on Robotics and Automation, 2010. [4] O. Khatib, “A unified approach for motion and force control of robot manipulators: The operational space formulation.” IEEE Journal of Robotics and Automation., vol. 3, no. 1, pp. 43–53, 1987. [5] J. Nakanishi, R. Cory, M. Mistry, J. Peters, and S. Schaal, “Operational space control: a theoretical and emprical comparison,” International Journal of Robotics Research, vol. 27, no. 6, pp. 737–757, 2008. [6] J. Peters, M. Mistry, F. E. Udwadia, J. Nakanishi, and S. Schaal, “A unifying methodology for robot control with redundant DOFs,” Autonomous Robots, no. 1, pp. 1–12, 2008. [7] L. Sentis, “Synthesis and control of whole-body behaviors in humanoid systems,” Ph.D. dissertation, Stanford University, July 2007. [8] F. Aghili, “A unified approach for inverse and direct dynamics of constrained multibody systems based on linear projection operator: Applications to control and simulation,” IEEE Transactions on Robotics, vol. 21, no. 5, pp. 834–849, October 2005. [9] J. Buchli, M. Kalakrishnan, M. Mistry, P. Pastor, and S. Schaal, “Compliant quadruped locomotion over rough terrain,” in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2009, pp. 814–820. [10] F. Udwadia and R. Kalaba, “On the foundations of analytical dynamics,” Int. J. of Non-Linear Mechanics, vol. 37, pp. 1079–1090, 2002. [11] A. Ben-Israel and T. Greville, Generalized inverses: theory and applications. Springer-Verlag New-York Inc., 2003. [12] L. Sentis, J. Park, and O. Khatib, “Compliant control of multi-contact and center of mass behaviors in humanoid robots,” IEEE Transactions on Robotics, vol. 26, no. 3, pp. 483–501, 2010.
I. INTRODUCTION One of the long standing goals of robotics research is to develop robots that can autonomously, dexterously and safely interact outside of carefully designed artificial environments. For instance, they would be able to operate in places where dextrous and safe interaction with humans is required, or in highly complex terrains where agility and robust performance in light of many disturbances is in the foreground (e.g. disaster sites). The development of such robots needs control algorithms that ensure high tracking performance and compliance at the same time in order to provide agile motion and dextrous manipulation while guaranteeing compliant contact interaction and safe interaction with humans. Traditional position control schemes are not suitable for such purposes since they inherently require high gains and therefore high stiffness for good tracking performance. In contrast, torque control strategies have recently been successfully implemented for the control of complex, redundant robots [1]–[3]. Especially model based approaches such as inverse dynamics controllers or operational space controllers are very appealing, as, they allow to significantly lower the gains and therefore allow for more compliant control while also guaranteeing high tracking performance. The general idea behind these model-based control scheme is to use a dynamic model of the robot in order to compute the necessary torques to achieve a desired movement. In addition to this feedforward command an error feedback command (e.g. a PID controller) with low gains is added This research was supported in part by National Science Foundation grants ECS-0326095, IIS-0535282, IIS-1017134, CNS-0619937, IIS0917318, CBET-0922784, EECS-0926052, the DARPA program on Advanced Robotic Manipulation, the Army Research Office, the Okawa Foundation, the ATR Computational Neuroscience Laboratories and the Swiss National Science Foundation. L. Righetti and S. Schaal, Computational Learning and Motor Control Lab, University of Southern California Los Angeles, CA90089, USA [email protected], [email protected] Jonas Buchli, Dept. of Advanced Robotics, Italian Institute of Technology, Genoa, Italy [email protected] Michael Mistry, Disney Research Pittsburgh, Pittsburgh, PA15213, USA [email protected]
to handle movement errors coming either from model uncertainty, parameter estimation errors, or external disturbances. Traditionally we can distinguish two control approaches for fully actuated unconstrained robots (typical examples are manipulators fixed on the ground). The first approach is to directly design joint trajectories and use an inverse dynamics controller to compute the necessary torques to achieve the desired motion and a PID controller with low gains in joint space for disturbance rejection. The second approach is to design task-space trajectories (e.g. the endeffectors) and use an operational space controller to compute the torques necessary to achieve the movement in task space. The error feedback is also designed in task-space coordinates. Additionally a command is generally added to optimize a desired posture in the nullspace of the task. There are several different approaches to design operational space controllers. One of the best known approach is the one introduced by Khatib [4] which has many interesting properties such as decoupling between task and nullspace forces and accelerations. An overview of these approaches together with experimental comparisons were presented in [5]. Recently [6] proposed a unified view where operational space control is formulated as a special class of optimal tracking controllers. Each approach to operational space control corresponds to a tracking controller that is instantaneously optimal with respect to a particular quadratic cost in the commands. However legged robots are inherently different from manipulators since they are underactuated systems due to their floating-base1 . Moreover these robots are subjected to switching contact constraints. Therefore legged robots cannot be directly controlled with the well-known control schemes described above. From the control point of view, we can distinguish three cases: • Legged robots can be seen as fully actuated when there are as many constraints as free floating DOFs (e.g. a humanoid is standing on one foot). The dynamic model can be reduced to a fixed-base manipulator model. • Legged robots can be underactuated when there are less constraints than free floating DOFs (e.g. a humanoid jumping in the air). • Legged robots can be overactuated when there are more constraints than free floating DOFs (e.g. a humanoid with two feet on the ground) and there exists an infinite choice of possible torques to achieve the desired constraint-consistent motion. It is possible to control such robots using different models and controllers for each constraint situation. However this approach is not practical because it requires as many models and controllers as constraint situations. Moreover each model 1 In order to generally describe the dynamics of a legged robot one has to add to the n degrees of freedom of the joints 6 degrees of freedom corresponding to the position and orientation of the robot relative to an inertial frame.
is derived in a constraint consistent subspace without explicitly computing these constraints. Therefore they are often not well suited for the control of contact constraints. It is also desirable to avoid measuring constraint forces since it would require sensors at each contact point and can lead to brittle control due to noise measurement and delay due to filtering. The ideal situation would be to have a single controller for all these different cases that can handle switching external contact constraints and that does not require constraint forces measurement. Recently such an approach has been taken for both operational space and joint position control of legged robots. Sentis et al. [1], [7] proposed an extension of operational space controllers for floating-base robots and for prioritization of tasks that requires only one dynamic model of the robot and that can handle various types of external constraints. They design a hierarchy of constraint-consistent tasks (e.g. movement of the center of mass and movement of the hands), each task acting in the nullspace of both higher priority tasks and the constraints. This approach is very appealing since it virtually allows one to control any desired position or force in task space and to superpose these tasks together for floatingbase robots under constraints. However one caveat of this approach is the complexity of the controller. Indeed among all of the needed computations, it requires the inversion of the rigid body inertia matrix in several places making this approach sensitive to modelling and parameter estimation errors [5]. To the best of our knowledge this approach was never applied on a real robotic platform. Another approach for joint space trajectory control was recently proposed by Aghili in [8], based on orthogonal projections. The inverse dynamics is computed by projecting the dynamics into a constraint consistent manifold thus removing the constraints. The advantage of such an approach is that it can also handle constraint switching and is computationally efficient since it is based on the decomposition of the constraint Jacobian, which is a purely kinematic quantity. More recently, based on similar ideas, Mistry et al. [3] proposed an inverse dynamics algorithm for legged robots with switching contact constraints. This algorithm is very appealing since it does not require the inversion of the inertia matrix and requires only kinematic terms and an estimation of the unconstrained inverse dynamics, which is computationally inexpensive. In addition it allows to predict the constraint forces efficiently using only kinematic projections, which can be used to develop force controllers [9]. This controller was also successfully applied on real robotic platforms demonstrating its robustness and efficiency for real-time control [3], [9]. The controllers mentioned above were independently derived for very different purposes and the control laws have very different forms. Several questions arise. To what extend are these control laws different? Is it possible to benefit of the advantages of the hierarchical task controllers while preserving the simplicity and robustness of the inverse dynamics controller based on orthogonal projections? Is it mandatory to use the inverse of the inertia matrix for the operational space approach? In this paper, we demonstrate the surprising insight that these previous different approaches to floating base inverse dynamics control can indeed be thought as equivalent. In order to do so, we develop a general framework to describe such controllers and prove the equivalence of all these controllers. This general framework for inverse dynamics
control of under-actuated robots under constraints is the main contribution of this paper. The consequences of this result are two-fold. First, we give a strong argument in favor of the use of orthogonal projections for inverting the dynamics [3], [8] since these approaches are equivalent to the ones using dynamic quantities and are easier to implement on real robotic systems. Second, we are able to simplify the controller presented in [1], [7] to work with kinematic projections and extend it to be optimal with respect to any quadratic cost in the commands. In Section 2 we present the general framework that will allow us to compare the different approaches and derive the main result of the paper. In Section 3 we discuss the practical consequences of this result, present the controllers that we wish to compare and simplify the operational space controller. We then show numerical simulations to support the mathematical results in Section 4 and discuss the computational aspects of the new results and their applicability on real platforms. We then conclude the paper. II. GENERAL FRAMEWORK A. Problem formulation The dynamics of a free-floating rigid-body robot subject to external constraints is generally given by2 M¨ q + h = ST τ + JTc λ
(1)
under the k constraints ¨ = b(q, q) ˙ Jc q
(2)
(n+6)×(n+6)
where M ∈ R is the rigid body dynamics (RBD) Inertia matrix, h ∈ Rn+6 is a generalized force vector containing the Coriolis, centrifugal and gravitational effects, τ ∈ Rn is the actuation vector and S ∈ Rn×(n+6) is the joint selection matrix that reflects the underactuation – for instance, for most floating base robots, S would be the identity matrix in the first n × n submatrix, and zeros elsewhere. Jc ∈ Rk×(n+6) is the Jacobian of the k constraints with the λ ∈ Rk Lagrange multipliers that correspond to the constraint forces. Following the ideas from [10] we express the constraints in acceleration form (Eq. 2) . Holonomic constraints can be expressed by differentiating them twice and non-holonomic constraints by differentiating them once. Without loss of generality, we assume that Jc is full row rank, in the sense that all constraints are linearly independent. Indeed it is always possible to find a reduced number of independent constraints, for example by using the SVD decomposition of Jc . Therefore our results will hold for an arbitrary number of constraints. For example, assume that the position of the point feet of a legged robot are given by xc , then the constraints that the feet do not move relative to the ground can be written as xc = const or equivalently by x˙ c = 0. Relating this to the motion of the joints of the robot using the Jacobian of xc we have x˙ c = Jc q˙ = 0, which we differentiate once again ¨ = −J˙ c q. ˙ to get Jc q We assume that the movement plan of the robot is expressed by desired joint accelerations that are constraint 2 We discuss floating base systems with n DOFS and n actuators, but all the derived results here can be directly applied to systems with a higher number of actuators (e.g. muscle-like systems) and to systems with a higher degree of under-actuation (e.g. a robot with passive elements). These cases are not included to keep notation and discussion as simple as possible.
¨ = q ¨ d holds. These consistent, i.e., Equation (2) with q accelerations will be satisfied if and only if they are of the form G ¨ d = JG q q0 (3) c b + (I − Jc Jc )¨ where JG c can be any generalized inverse [11] of Jc , i.e. a ¨ 0 is an arbitrary acceleramatrix such that Jc JG c Jc = Jc . q tion vector and (I − JG J ) projects these accelerations into c c the null space of the constraints. The general problem of inverse dynamics is to compute the torques τ such that they will achieve the desired accel¨ d . This is equivalent to solving Equations (1) and erations q ¨=q ¨ d . The problem is not straightforward (2) for τ with q since the system is underactuated due to the floating base and its dynamics must satisfy constraints. B. Class of possible controllers The solutions of the inverse dynamics problem will essentially live on a manifold embedded in Rn+6 where the constraints are enforced (i.e. when restricted to this manifold the dynamics can be written without Lagrange multipliers). Lets define a linear operator P(q) of rank n + 6 − k that maps the dynamics onto this constraint consistent manifold such that the dynamics (i.e. the free dynamics) is written as P(M¨ qd + h) = PST τ
in the commands at each instant of time. First we state the following lemma (also discussed in [8]) ¨ d are achievable, Lemma 1: If the desired accelerations q all possible controllers for the inverse dynamics problem under constraints that achieve perfect tracking of these desired accelerations can be written in the form of τ (W, τ 0 )
W
+ (I − (PST ) PST )W−1 τ 0 with
W
(PST )
(5)
¨ , we can and by noting that the constraints enforce b = Jc q insert the constraints into Eq. (1) and we get an equation that it is equivalent to choosing the linear operator Pdirect = (I − JTc (Jc M−1 JTc )−1 Jc M−1 )
(6)
to describe the unconstrained dynamics. Therefore there always exists one such operator. Solving the original problem is then reduced to solving Equation (4). We can already see that the number of solutions to this equation will depend on the number of constraints. Indeed the unconstrained dynamics lies in a n + 6 − k dimensional manifold while the dimension of the control vector is n. There are then 3 distinct cases: • k < 6, the system is underactuated. There is at most one solution to the inverse dynamics problem: for a solution to exist, the desired accelerations must not only be constraint consistent, they moreover need to be consistent with the dynamics of Eq. (1). For example one can think of the case without constraints: a falling cat cannot orient its body and move its joints independently. • k = 6, the system is fully actuated. There is exactly one solution provided that the desired accelerations are constraint consistent. As we already discussed in the introduction this case is similar to the inverse dynamics problem of a manipulator fixed to the ground. • k > 6, the system is overconstrained. There is an infinite ¨ d . It is number of solutions for τ that will achieve q the case, for example, when a humanoid has its two feet on the ground (12 constraints) or when a point feet quadruped robot has 3 feet on the ground (9 constraints). In the following we show that in the overconstrained case we can create controllers that are optimal for a quadratic cost
1
(8)
where () denotes the Moore-Penrose generalized inverse [11] , W is symmetric positive definite3 and τ 0 is an arbitrary torque vector. Moreover the unique controller that minimizes the cost τ T Wτ can be written as W
τ (W, 0) = (PST ) P(M¨ qd + h)
(9)
Proof: The inverse dynamics problem with constraints is equivalent to solving Equation (4). Since W is symmetric 1 positive definite, its square root W 2 exists and is also symmetric positive definite and therefore invertible. We can then write Equation (4) as 1
1
PST W− 2 W 2 τ = P(M¨ qd + h) and then we get the general solution to this equation as 1
λ = (Jc M−1 JTc )−1 (b + Jc M−1 (h − ST τ ))
1
= W− 2 (PST W− 2 )+
(7)
+
(4)
For example if we compute the Lagrange multipliers that are uniquely given by [10]
W
= (PST ) P(M¨ qd + h)
W2τ
=
1
(PST W− 2 )+ P(M¨ qd + h) 1
1
τ0 + (I − (PST W− 2 )+ PST W− 2 )˜ where τ˜ 0 is an arbitrary torque. Therefore we can write it 1 as τ˜ 0 = W− 2 τ 0 and the first part of the lemma follows from standard computations. Since the unique solution to an equation of the form Px = b that minimizes kxk is 1 1 x = P+ b, then τ (W, 0) minimizes kW 2 τ k. Since W 2 is 1 symmetric kW 2 τ k2 = τ T Wτ . This proves the second part of the lemma. Thus given a formulation of constraint consistent dynamics (i.e. the general dynamics restricted to a feasible manifold), we have generated a class of controllers parametrized by W and τ 0 . This result has to be compared to the work of [6] where the authors derive the class of operational space controllers that optimize a similar cost function for fully actuated systems. The main difference from our result is that we characterize underactuated systems with constraints for joint space tracking. Remark Multiple solutions for inverse dynamics control only exist in the overconstrained case, i.e. when there are more constraints than degrees of underactuation. Therefore the parameters W and τ 0 are associated with redundancy resolution at the torque level and will have an effect only when k > 6. In the case k ≤ 6 there is at most one solution to the problem and all parametrizations will lead to the same controller. W
Remark Note that (PST ) is a generalized inverse of W PST with a weight W. Indeed PST (PST ) PST = PST . Moreover using the properties of Moore-Penrose inverses we 1 3W 2
is the square root of the positive definite matrix W, i.e. the unique 1 1 positive definite matrix such that W 2 W 2 = W
can always rewrite this generalized inverse such that it does not require to compute the square root of a matrix W
(PST )
= W−1 SPT (PST W−1 SPT )+
(10)
torque control law for tracking desired joint trajectories qd is then given by τ
1
P1 ST τ 1 P2 ST τ 2
= =
P1 (M¨ qd + h) P2 (M¨ qd + h)
(11) (12)
with their parametrized set of possible controllers τ 1 (W, τ 0 ) and τ 2 (W, τ 0 ). The following equality holds τ 1 (W, τ 0 ) = τ 2 (W, τ 0 ) (13) Remark This results shows that the controllers can differ only in the way the torque redundancy resolution is done (i.e. the choice of W and τ 0 ). The choice of Pi does not create new controllers and is only important from a computational point of view. This constitutes a strong argument for the use of the simplest projection possible that uses only kinematic parameters to compute the controller. III. PRACTICAL CONSEQUENCES We now discuss the consequences of the previous results for the controllers we described in the Introduction. A. Constraint elimination via orthogonal projections Mistry et al. [3] recently proposed an inverse dynamics control law based on the orthogonal decomposition of the constraint Jacobian. It was originally designed to handle fully or overconstrained cases and holonomic constraints. Here we rewrite their controller and note that this controller can in fact handle any types of constraints written in the form of ¨ d is Eq. (2) and works also in the underactuated case (if q achievable), since the Moore-Penrose inverse always exist. Assume the QR decomposition of the constraint Jacobian as JTc = Q[RT 0]T . We can multiply Equation (1) by QT and get the following decoupled dynamics Su QT ST τ λ
= Su QT (M¨ qd + h) (14) = R−1 Sc QT (M¨ qd + h − ST τ ) (15)
where Su = [0(n+6−k)×k I(n+6−k)×(n+6−k) ] selects the columns of QT corresponding to the unconstrained space and Sc = [I(n+6−k)×k 0(n+6−k)×(n+6−k) ] selects the constrained dimensions. The inverse dynamics controller follows if we take PQR = Su QT (16) In this case the linear operator PQR is a purely kinematic operator since it uses only the constrained Jacobian. This formulation is very simple and practical since it merely requires a kinematic projection and estimation of the unconstrained inverse dynamics. Moreover it gives an equation for the constraint forces that is very simple when compared to Eq. (5) and can be used to develop force controllers [9]. The
1
1
+ (I − W− 2 (PQR ST W− 2 )+ PQR ST )W−1 τ 0 + PID (qd , q˙ d ) (17)
C. Invariance of the controllers under projection choice We use the previous formulation to state the following lemma, which is one of the main contributions of this paper (the proof is given in the appendix) Lemma 2: Given two different linear operators P1 and P2 that describe the same system with constraints
1
= W− 2 (PQR ST W− 2 )+ PQR (M¨ qd + h)
B. Operational space control The operational space formulation derived by Khatib [4] was recently extended to include task hierarchies on floating base systems by Sentis et al. [1], [7] and to control the center of pressure of contacts using the nullspace torques (using τ 0 in our framework) [12]. The controller that is presented in [7], [12] is written as τ
=
(SNc ST )+ SNc ! N X T T × (Js|prec(s) Fs ) + Nt τ posture s=1
+ (I − (SNc ST )+ (SNc ST ))τ 0
(18)
with Nc = M−1 I − JTc (Jc M−1 JTc )−1 Jc M−1
(19)
where JTs|prec(s) are prioritized Jacobians of prioritized operational space tasks, Fs are dynamically consistent control forces, NTt is the cumulative prioritized null space matrix associated with higher priority tasks and τ posture is a postural control vector. If we write ! N X ¨ d = M−1 q (JTs|prec(s) Fs ) + NTt τ posture − h (20) s=1
then this controller can be thought as an inverse dynamics controller and therefore be cast in our framework by selecting Pop.sp. = SNc
(21)
where W = I. We note that in the absence of internal torques τ 0 = 0, the original controller used in [7] minimizes the cost τ T τ , since W = I. Using the results presented here we can generalize this controller to minimize any quadratic cost in the commands by selecting a different W. Second we note that Nc ∈ R(n+6)×(n+6) is a relatively complex terms where there are 3 occurrences of the inverse of the inertia matrix. In the overconstrained case SNc ST will be rank deficient, requiring special numerical methods to compute the MoorePenrose inverse while it will not be the case for PQR . Since we have shown that different linear operators lead to the same controllers in Lemma 2, we can replace Pop.sp. by PQR and get a simpler but equivalent whole body controller τ
1
1
= W− 2 (PQR ST W− 2 )+ PQR ! N X T T × (Js|prec(s) Fs ) + Nt τ posture s=1
+ (I − W
− 21
1
(PQR ST W− 2 )+ PQR ST )W−1 τ 0 (22)
where we removed the unnecessary appearances of the inertia matrix. The reformulation of the controller is interesting as it greatly simplifies the computation of the controller using only kinematic parameters for the projection. Moreover this
controller generally minimize any cost τ T Wτ when we choose τ 0 = 0 and as such represents a generalization and simplification of the previous approach. IV. EXPERIMENTS In the following we present numerical evaluations to confirm the results presented in the previous sections. We also discuss the computational aspects of the various controllers and therefore their applicability on real robotic platforms. A. Description of the controllers In the following experiments we use the three linear operators discussed in this paper. First the operator that arises when computing the Lagrange multipliers as we showed in Eq. (6) Pdirect = (I − JTc (Jc M−1 JTc )−1 Jc M−1 )
(23)
Second, the operator resulting from the QR decomposition of the constraint Jacobian PQR = Su QT
(24)
Finally, the operator derived from the whole-body controllers using the operational space formalism Pop.sp. = SM−1 I − JTc (Jc M−1 JTc )−1 Jc M−1 (25) In order to show the generalization of the controllers to minimize any quadratic cost in the commands, we define three different costs. 1 We minimize total torque τ T τ by selecting W = I 2 We into account actuator saturation and minimize P take τi 2 i ( τ i,max ) where τ i,max is the maximum torque al1 lowed for actuator i. In this case W = diag{ (τ i,max )2 } 3 We use the inertia matrix as metric, which is a popular metric for manipulators [4], [6] and choose W = SM−1 ST , which corresponds to the inverse of the joint space inertia matrix. Note that in every case we use τ 0 = 0. The desired accelerations are computed using an operational space controller for the position of the base of the robot (i.e. its trunk) together with a control of a default posture for the joints in the nullspace. The full state constraint consistent desired ¨ d are computed as accelerations q ¨d q
¨ posture = −Jc q˙ + (Jb Nc )+ x ¨d + (SNb )+ q
x ¨d
=
q ¨posture
Kbp (xd − x) + Kbd (x˙ d − x) ˙ −
d(Jb Nc ) x˙ dt
= Kjp (qposture − qj ) − Kjd q˙ j −
d(SNb ) q˙ j dt
where Nc = I − J+ c Jc is the nullspace projector of the constraint Jacobian, Jb is the Jacobian of the base position and Nb = I − (Jb Nc )+ Jb Nc is the nullspace projector of the constraint consistent Jacobian Jb Nc . The actual and desired position of the base and the joints current and default positions are denoted by x, xd , qj and qpost respectively. The matrices Ki are positive definite gain matrices. We perform the tests on a physics simulation of a biped humanoid that is modeled according to the Sarcos Humanoid, which is a 34 DOFs human-sized humanoid robot (Fig. 1).
Fig. 1.
Picture of the robot used for the simulations
B. Results and computational considerations The controllers perform a sine movement on the lateral coordinate of the base (i.e. moving its weight from left to right) and a sine movement in the vertical coordinate (i.e. moving up and down) while keeping the position of the sagittal coordinate constant. Figure 2 shows the result of the simulations. As expected we see that the controllers differ only from the redundancy resolution schemes, W, and that the choice of the linear operator P does not change anything. We note that the redundancy resolution leads to very different torques distributions and as expected the minimum costs are attained when choosing the appropriate weights. From a practical point of view, the choice of the linear operator will be critical in order to create controllers that can be run in real-time and that are numerically stable. We have shown that the use of the inertia matrix was not important and constituted an additional complexity for numerical computation4 . On the other hand computing the QR decomposition of the constraint Jacobian is extremely fast on modern computers by using a SVD decomposition and can be used in real-time control loops. From these considerations, this results constitute a strong argument for the use of kinematic projections for inverse dynamics controllers and operational space controllers. The new formulation of the whole-body controller of [1], [7] constitutes a real improvement from the computational point of view for practical implementations. V. CONCLUSION In an attempt to compare different control approaches for inverse dynamics and operational space control for floating base robots, we developed a general framework for inverse dynamics control of underactuated systems under constraints. We derived the class of possible controllers for inverse dynamics together with optimality results for any quadratic cost in the commands. We then showed that recent approaches for operational space control [1], [7], [12] and inverse dynamics [3] were in fact equivalent. From these results we were able to show that the wholebody controller proposed by [1] could be thought as an inverse dynamics controller. We were then able to greatly simplify it using kinematic quantities. We also generalized this controller such that we can optimize any quadratic cost in the commands. Since the linear operators used for the inverse dynamics is theoretically not important, it constitutes a strong argument for using operators based on kinematic quantities as they are simpler to use and already proved to be robust when applied to real robotic systems [3], [9]. It also shows that the use of the inverse of the inertia 4 Note that our result also shows that theoretically the use of a badly estimated inertia matrix does not change anything. However this could be problematic from a numerical point of view.
2
4
6
8
10
Base y
0 ï0.02 ï0.04 0
2
4
6
8
10
Base z
0 ï0.05 ï0.1 0
2
4
Time [s]
6
8
10
2000 1000 0
2
4
6
8
10
4
x 10 8 6 4 2 0
15 10 5 0 ï5 0
2
4
6
8
10
3 2 1 0 0
Left Hip Flex/Ext Torque [N.m]
ï0.05 0
20
3000
2
4
Time [s]
6
8
10
Left Ankle Ad/Abd Torque [N.m]
Total Torque Cost
0
Torque Inertia Weighted Saturation Cost Cost
Base x
0.05
2
10
4 6 control 8 Total Torque Inertia weighted control Actuator Saturation control
10
5 0 ï5
ï10 0
2
4
Time [s]
6
8
10
Fig. 2. Result of the experiments described in Section IV. The left figure shows the movement of the base for the different controllers. The middle figure shows the instantaneous value of the different costs we optimize and the right one shows the torques applied. The results from all 9 controllers are superposed (3 projections Pi tested with 3 redundancy resolution schemes). The redundancy resolution corresponding to optimization of τ T τ is in black dashed/dotted line, the one corresponding to τ T SM−1 ST τ is in dashed blue line and the one corresponding to minimization of actuator saturation is in plain red line. All the three controllers derived from different projections Pi are superposed. The left figure shows that all the controllers produce the same task space motion. The middle and right figures show that the choice of W leads to different commands while the choice of P does not change anything since the commands are superposed and cannot be distinguished.
matrix does not provide any theoretical advantages for these types of control. From a larger perspective, these results are interesting since they provide a unified framework for comparing very different controllers and to derive optimality results. A PPENDIX P ROOF OF L EMMA 2 The two controllers τ 1 (W, 0) and τ 2 (W, 0) must be equal since they both are controllers that achieve perfect tracking of the desired accelerations and minimize the cost τ T Wτ while there exist only one controller that minimizes this cost because of Lemma 1. Therefore we have τ 1 (W, 0) = τ 2 (W, 0)
(26)
which implies (P1 ST W
− 12
)+ P1 (M¨ qd +h) = (P2 ST W
− 12
)+ P2 (M¨ qd +h) (27) Since both Equations (11) and (12) describe the same constraint system, their sets of possible controllers are the same. τ 1 is then a solution to P2 ST τ 2 = P2 (M¨ qd + h) and we can write T P2 (M¨ qd + h) = P2 S τ 1 1
1
we multiply by P2 ST W− 2 (P2 ST W− 2 )+ to get 1
1
P2 ST W− 2 (P2 ST W− 2 )+ P2 ST τ 1 1
1
qd + h) = P2 ST W− 2 (P2 ST W− 2 )+ P2 (M¨ = P2 ST W
− 12
1
(P1 ST W− 2 )+ P1 (M¨ qd + h)
where the last equality is found by using Eq. (27). Developping τ 1 using Eq. (7) and simplifying we finally get 1 1 1 1 P2 ST W− 2 I − (P1 ST W− 2 )+ P1 ST W− 2 W− 2 τ 0 = 0 (28) This equation means that any vector that is in the nullspace 1 1 of P1 ST W− 2 is also in the nullspace of P2 ST W− 2 . Conversely we can derive a similar equation that will show 1 that any vector in the nullspace of P2 ST W− 2 is also in T − 12 the nullspace of P1 S W . These nullspaces are then the same. Now we look at the difference of the controllers τ 1 and τ 2 . After simplification we get 1 1 1 τ1 − τ2 = W− 2 (P1 ST W− 2 )+ P1 ST W− 2 1 1 1 −(P2 ST W− 2 )+ P2 ST W− 2 W− 2 τ 0
Inserting Equation (28) we can then find 1 1 1 τ 1 − τ 2 = W− 2 (P2 ST W− 2 )+ P2 ST W− 2 − I 1 1 1 × (P1 ST W− 2 )+ P1 ST W− 2 W− 2 τ 0 (29) 1 1 Since (P1 ST W− 2 )+ P1 ST W− 2 projects in the orthogonal 1 complement of the null space of P1 ST W− 2 then it also projects in the orthogonal complement of the null space of 1 P2 ST W− 2 and as a consequence the right hand side of Eq. (29) must be zero. This proves that for a given set of W and τ 0 , the two controllers τ 1 and τ 2 are the same and this completes the proof. R EFERENCES [1] L. Sentis and O. Khatib, “Synthesis of whole-body behaviors through hierarchical control of behavioral primitives,” International Journal of Humanoid Robotics, vol. 2, no. 4, pp. 505–518, 2005. [2] S. Hyon, J. G. Hale, and G. Cheng, “Full-body compliant humanhumanoid interaction: Balancing in the presence of unknown external forces,” IEEE Trans. on Robotics, vol. 23, no. 5, pp. 884–898, 2007. [3] M. Mistry, J. Buchli, and S. Schaal, “Inverse dynamics control of floating base systems using orthogonal decomposition,” in In proc. of the 2010 International Conference on Robotics and Automation, 2010. [4] O. Khatib, “A unified approach for motion and force control of robot manipulators: The operational space formulation.” IEEE Journal of Robotics and Automation., vol. 3, no. 1, pp. 43–53, 1987. [5] J. Nakanishi, R. Cory, M. Mistry, J. Peters, and S. Schaal, “Operational space control: a theoretical and emprical comparison,” International Journal of Robotics Research, vol. 27, no. 6, pp. 737–757, 2008. [6] J. Peters, M. Mistry, F. E. Udwadia, J. Nakanishi, and S. Schaal, “A unifying methodology for robot control with redundant DOFs,” Autonomous Robots, no. 1, pp. 1–12, 2008. [7] L. Sentis, “Synthesis and control of whole-body behaviors in humanoid systems,” Ph.D. dissertation, Stanford University, July 2007. [8] F. Aghili, “A unified approach for inverse and direct dynamics of constrained multibody systems based on linear projection operator: Applications to control and simulation,” IEEE Transactions on Robotics, vol. 21, no. 5, pp. 834–849, October 2005. [9] J. Buchli, M. Kalakrishnan, M. Mistry, P. Pastor, and S. Schaal, “Compliant quadruped locomotion over rough terrain,” in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2009, pp. 814–820. [10] F. Udwadia and R. Kalaba, “On the foundations of analytical dynamics,” Int. J. of Non-Linear Mechanics, vol. 37, pp. 1079–1090, 2002. [11] A. Ben-Israel and T. Greville, Generalized inverses: theory and applications. Springer-Verlag New-York Inc., 2003. [12] L. Sentis, J. Park, and O. Khatib, “Compliant control of multi-contact and center of mass behaviors in humanoid robots,” IEEE Transactions on Robotics, vol. 26, no. 3, pp. 483–501, 2010.
