#  Back-to-back azimuthal correlations in $\rm{Z}+$jet events at high
transverse momentum in the TMD parton branching method at next-to-leading
order

H. Yang  School of Physics, Peking University  Deutsches Elektronen-
Synchrotron DESY, Germany  A. Bermudez Martinez  Deutsches Elektronen-
Synchrotron DESY, Germany  L.I. Estevez Banos  Deutsches Elektronen-
Synchrotron DESY, Germany  F. Hautmann  CERN, Theoretical Physics Department,
Geneva  Elementary Particle Physics, University of Antwerp, Belgium
University of Oxford, UK  H. Jung  Deutsches Elektronen-Synchrotron DESY,
Germany  M. Mendizabal  Deutsches Elektronen-Synchrotron DESY, Germany  K.
Moral Figueroa  University of Edinburgh, UK  S. Prestel  Department of
Astronomy and Theoretical Physics, Lund University, Sweden  S. Taheri Monfared
Deutsches Elektronen-Synchrotron DESY, Germany  A.M. van Kampen  Elementary
Particle Physics, University of Antwerp, Belgium  Q. Wang  Deutsches
Elektronen-Synchrotron DESY, Germany  School of Physics, Peking University  K.
Wichmann  Deutsches Elektronen-Synchrotron DESY, Germany

###### Abstract

Azimuthal correlations in $\rm{Z}+$jet production at large transverse momenta
are computed by matching Parton - Branching (PB) TMD parton distributions and
showers with NLO calculations via MCatNLO. The predictions are compared with
those for dijet production in the same kinematic range. The azimuthal
correlations $\Delta\phi$ between the Z boson and the leading jet are steeper
compared to those in dijet production at transverse momenta ${\cal O}(100)$ ,
while they become similar for very high transverse momenta ${\cal O}(1000)$ .
The different patterns of $\rm{Z}+$jet and dijet azimuthal correlations can be
used to search for potential factorization - breaking effects in the back-to-
back region, which depend on the different color and spin structure of the
final states and their interferences with the initial states. In order to
investigate these effects experimentally, we propose to measure the ratio of
the distributions in $\Delta\phi$ for $\rm{Z}+$jet\- and multijet production
at low and at high transverse momenta, and compare the results to predictions
obtained assuming factorization. We examine the role of theoretical
uncertainties by performing variations of the factorization scale,
renormalization scale and matching scale. In particular, we present a
comparative study of matching scale uncertainties in the cases of PB-TMD and
collinear parton showers.

DESY-22-025

##  1 Introduction

The description of the cross section of high $p_{\rm T}$ jets in association
with a Z-boson at high $p_{\rm T}$ in proton-proton ($\rm{p}\rm{p}$)
collisions is an important test of predictions obtained in Quantum
Chromodynamics (QCD), and provides an important background to Higgs boson
studies and to new physics searches. At leading order in the strong coupling
$\alpha_{\mathrm{s}}$, the azimuthal angle $\Delta\phi$ between the Z boson
and the jet is $\Delta\phi=\pi$, and a deviation from this back-to-back
scenario is a measure of higher order radiation. In multijet events the
azimuthal correlation between two jets has been measured at the LHC by ATLAS
and CMS [1, 2, 3, 4, 5]. The production of Z bosons associated with jets has
been measured at lower energies, by CDF and D0 in proton-antiproton
($\rm{p}\rm{\mathrm{bar}p}$) collisions at a center-of-mass energy
$\sqrt{s}=1.96$ [6, 7]. At the LHC the ATLAS and CMS collaborations have
published measurements in $\rm{p}\rm{p}$ collisions at a center-of-mass energy
$\sqrt{s}=7$ [8, 9, 10], 8 [11] and 13 [12, 13]. The azimuthal correlation
between Z bosons and jets has been measured at 8 [11] and 13 [13]. However,
all the measurements of azimuthal correlations were performed at rather low
transverse momenta of the Z boson and the jets ($p_{\rm T}<{\cal O}(100)$ ),
where multiparton emissions are significant and next-to-leading-order (NLO)
calculations of $\rm{Z}+$jet are not sufficient to describe the measurement.
With the increase in luminosity at the LHC, it becomes possible to measure
$\rm{Z}+$jet production in the high $p_{\rm T}$ range, with $p_{{\rm T},Z}\gg
100$ . Especially the back-to-back region can be studied in detail, which
gives important information on soft gluon resummation and effects of the
transverse momenta of the initial partons in the form of transverse momentum
dependent (TMD) parton distributions (PDFs).

In a previous publication [14] we have investigated the $\Delta\phi_{12}$
correlation in high-$p_{\rm T}$ dijet events by applying TMD PDFs and parton
shower together with NLO calculations of the hard scattering process. The
application of TMD PDFs allows a direct investigation of initial state parton
radiation (an overview on TMD PDFs is given in Ref. [15]). While hard
perturbative higher order radiation leads to a large azimuthal decorrelation
(${\Delta\phi_{12}}\ll\pi$), soft multi-gluon emissions, which cannot be
described by fixed order calculations, dominate in the region
${\Delta\phi_{12}}\to\pi$. The region of ${\Delta\phi_{12}}\to\pi$ is of
special interest, since so-called factorization - breaking [16, 17, 18]
effects could become important in the case of colored final states. Multijet
production is believed to be sensitive to such effects, as well as vector
boson + jet production [19]. In order to investigate factorization - breaking
effects, we propose to compare the theoretical description of the azimuthal
correlation $\Delta\phi_{12}$ in multijet production with the one in
$\rm{Z}+$jet production.

In this report we compare in detail high-$p_{\rm T}$ dijet and $\rm{Z}+$jet
production by applying the Parton Branching (PB) formulation of TMD evolution
[20, 21] together with NLO calculations of the hard scattering process in the
MadGraph5_aMC@NLO [22] framework. In Ref. [14] these PB TMD PDFs were applied
to multijet production at large transverse momenta. We apply the same method
to the calculation of $\rm{Z}+$jet production. We propose to use the same
kinematic region for the high-$p_{\rm T}$ dijet and $\rm{Z}+$jet production to
allow a direct comparison of the measurements. At large enough $p_{\rm T}$ the
mass of the Z-boson becomes negligible, and the different color and spin
structure of the final states might allow to observe factorization - breaking
effects by comparing the measurements to calculations assuming that
factorization holds.

In the following, we describe the basic elements of the $\rm{Z}+$jet
calculation in Sec. 2. In Sec. 3 we present results for the $\rm{Z}+$jet
azimuthal correlations and compare them with the multijet case. We summarize
in Sec. 4.

##  2 Calculation of $\rm{Z}+$jet distributions

The PB - formulation of TMD evolution is given in Refs. [20, 21], introducing
Sudakov form factors and the soft-gluon resolution scale $z_{M}$ to separate
resolvable and non-resolvable branchings. By integrating over transverse
momenta, the PB-TMD evolution equations coincide with the DGLAP [23, 24, 25,
26] evolution equation for $z_{M}\to 1$, while they coincide with the CMW [27,
28] coherent branching equation for finite, angular-ordered $z_{M}$ [29]. The
NLO PB collinear and TMD parton distribution were obtained in Ref. [30] from
QCD fits to precision DIS data from at HERA [31].

The process $\rm{Z}+$jet at NLO is calculated with MadGraph5_aMC@NLO using the
collinear PB-NLO-2018-Set 2, as obtained in Ref. [30] applying
$\alpha_{\mathrm{s}}(M_{\rm{Z}})=0.118$. The matching of NLO matrix elements
with PB TMD parton distributions is described in Refs. [32, 33, 34]. The
extension to multijet production is illustrated in Ref. [14]. Predictions are
obtained by processing the MadGraph5_aMC@NLO event files in LHE format [35]
through Cascade3 [34] for an inclusion of TMD effects in the initial state and
for simulation of the corresponding parton shower (labeled MCatNLO+CAS3 in the
following).

Fixed order NLO $\rm{Z}+$jet production is calculated with MadGraph5_aMC@NLO
in a procedure similar to the one applied for dijet production described in
[14] (labeled MCatNLO(fNLO)). For the MC@NLO mode, the Herwig6 [36, 37]
subtraction terms are calculated, as they are best suited for the use with PB
- parton densities, because both apply the same angular ordering condition.
The matching scale $\mu_{m}=\verb+SCALUP+$ limits the contribution from PB-
TMDs and TMD showers.

In the calculations the factorization and renormalization scales are set to
$\mu=\frac{1}{2}\sum_{i}p_{{\rm T},i}$, where the index $i$ runs over all
particles in the matrix element final state. This scale is also used in the
PB-TMD parton distribution ${\cal A}(x,k_{\rm T},\mu)$. The scale
uncertainties of the predictions are obtained from variations of the scales
around the central value in the 7-point scheme avoiding extreme cases of
variation.

Figure 1: Transverse momentum spectrum of the $\rm{Z}+$jet-system $p_{{\rm
T},Zj}$ (left) and ${\Delta\phi_{\rm{Z}\text{j}}}$ distribution (right). The
predictions are shown for fixed NLO (MCatNLO(fNLO) and after inclusion of PB-
TMDs (MCatNLO+CAS3).

In Fig. 1 we show the distributions of the transverse momentum of the
$\rm{Z}+$jet-system, $p_{{\rm T},Zj}$, and the azimuthal correlation in the
$\rm{Z}+$jet-system, $\Delta\phi_{\rm{Z}\text{j}}$, for a fixed NLO
calculation, as well as for the full simulation including PB-TMD PDFs and
parton showers. We require a transverse momentum $p_{\rm T}>200$ for the
Z-boson and define jets with the anti-$k_{\rm T}$ jet-algorithm [38], as
implemented in the FASTJET package [39], with a distance parameter of R=0.4.

In the low $p_{{\rm T},Zj}$-region one can clearly see the expected steeply
rising behavior of the fixed NLO prediction. In the
$\Delta\phi_{\rm{Z}\text{j}}$ distribution one can observe the limited region
for fixed NLO at ${\Delta\phi_{\rm{Z}\text{j}}}<2/3\pi$, since at most two
jets in addition to the Z-boson appear in the calculation. At large
$\Delta\phi_{\rm{Z}\text{j}}$, the fixed NLO prediction rises faster than the
full calculation including resummation via PB-TMDs and parton showers. In the
following we concentrate on the large $\Delta\phi_{\rm{Z}\text{j}}$ region.

##  3 Back-to-back azimuthal correlations in $\rm{Z}+$jet and multijet
production

We now present predictions, obtained in the framework described above, for
$\rm{Z}+$jet and multijet production.***A framework based on CCFM evolution
[40] was described in [41, 42] for multi-jet and vector boson + jet
correlations. The selection of events follows the one of the azimuthal
correlations $\Delta\phi_{12}$ in the back-to-back region
(${\Delta\phi_{12}}\to\pi$) in multijet production at $\sqrt{s}=13$ as
obtained by CMS [5]: jets are reconstructed with the anti-$k_{\rm T}$
algorithm [38] with a distance parameter of 0.4 in the rapidity range of
$|y|<2.4$. We require either two jets with $p_{\rm T}^{\text{leading}}>200$ or
a Z boson and a jet as leading or subleading objects with a transverse
momentum $p_{\rm T}^{\text{leading}}>200$ .

We consider distributions of the azimuthal correlation between the Z-boson and
the leading jet, $\Delta\phi_{\rm{Z}\text{j}}$, for $p_{\rm
T}^{\text{leading}}>200$ as well as for the very high $p_{\rm T}$ region of
$p_{\rm T}^{\text{leading}}>1000$ .

The calculations are performed with MCatNLO+CAS3 using PB-NLO-2018-Set 2 as
the collinear and TMD parton densities with running coupling satisfying
$\alpha_{\mathrm{s}}(m_{\rm{Z}})=0.118$ and PB-TMD parton shower.

Figure 2: Predictions of the azimuthal correlation
$\Delta\phi_{\rm{Z}\text{j}}$($\Delta\phi_{12}$) for $\rm{Z}+$jet and multijet
processes in the back-to-back region for $p_{\rm T}^{\text{leading}}>200$
(left) and $p_{\rm T}^{\text{leading}}>1000$ (right) obtained from
MCatNLO+CAS3. Shown are the uncertainties obtained from scale variation (as
described in the text). The measurements of dijet correlations as obtained by
CMS [5] are shown as data points, for comparison.

In Fig. 2 the prediction for the azimuthal correlations
$\Delta\phi_{\rm{Z}\text{j}}$ for $\rm{Z}+$jet production in the back-to-back
region is shown.†††Predictions for the region of small $\Delta\phi$ require
including the contribution of higher parton multiplicities, e.g. via multi-jet
merging [43]. We also show, for comparison, the prediction of azimuthal
correlations $\Delta\phi_{12}$ for multijet production in the same kinematic
region, compared to the measurement of dijet production obtained by CMS [5].
We observe that the distribution of azimuthal angle
$\Delta\phi_{\rm{Z}\text{j}}$ in $\rm{Z}+$jet-production for $p_{\rm
T}^{\text{leading}}>200$ is more strongly correlated towards $\pi$ than the
distribution of angle $\Delta\phi_{12}$ in multijet production. This
difference is reduced for $p_{\rm T}^{\text{leading}}>1000$ .

Figure 3: TMD parton density distributions for up quarks (PB-NLO-2018-Set 2)
as a function of $k_{\rm T}$ at $\mu=200$ and $1000$ and $x=0.01$.

Differences in $\Delta\phi$ between $\rm{Z}+$jet and multijet production can
result from the different flavor composition of the initial state and
therefore different initial state transverse momenta and initial state parton
shower, as well as from differences in final state showering since both
processes have a different number of colored final state partons. Effects
coming from factorization - breaking, interference between initial and final
state partons, will depend on the final state structure and the number of
colored final state partons.

We first investigate the role of initial state radiation and the dependence on
the transverse momentum distributions coming from the TMD PDFs, which gives a
large contribution to the decorrelation in $\Delta\phi$. The $k_{\rm
T}$-distribution obtained from a gluon TMD PDF is different from the one of a
quark TMD PDF as shown in Fig. 3 for $x=0.01$ and scales of $\mu=200(1000)$ .
In Fig. 4 we show the probability of $gg$, $qg$ and $qq$ initial states ($q$
stands for quark and antiquark) as a function of $p_{\rm T}^{\text{leading}}$
for $\rm{Z}+$jet and multijet production obtained with MCatNLO+CAS3. At high
$p_{\rm T}^{\text{leading}}>1000$ the $qq$ channel is dominant for both
$\rm{Z}+$jet and multijet final states, while at lower $p_{\rm
T}^{\text{leading}}>200$ the $gg$ channel is dominant in multijet production,
leading to larger decorrelation effects, since gluons radiate more compared to
quarks.

Figure 4: The probability of $gg$, $qg$ and $qq$ initial states in
$\rm{Z}+$jet and multijet production ($q$ stands for quark and antiquark) as a
function of $p_{\rm T}^{\text{leading}}$. The predictions are calculated with
MCatNLO+CAS3.

The role of final state radiation in the correlation in $\Delta\phi_{12}$
distributions is more difficult to estimate, since the subtraction terms for
the NLO matrix element calculation also depend on the structure of the final
state parton shower. In order to estimate the effect of final state shower we
compare a calculation of the azimuthal correlations in the back-to-back region
obtained with MCatNLO+CAS3 with the one obtained with MCatNLO+Pythia8 (Fig.
5). For the calculation MCatNLO+Pythia8 we apply the Pythia8 subtraction terms
in the MadGraph5_aMC@NLO calculation, use the NNPDF3.0 [44] parton density and
tune CUETP8M1 [45].

Figure 5: Predictions for the azimuthal correlation
$\Delta\phi_{\rm{Z}\text{j}}$($\Delta\phi_{12}$) in the back-to-back region
for $\rm{Z}+$jet and multijet production obtained with MCatNLO+CAS3 (left
column) and MCatNLO+Pythia8 (right column). Shown are different regions in
$p_{\rm T}^{\text{leading}}>200$ (upper row) and $p_{\rm
T}^{\text{leading}}>1000$ (lower row). The bands show the uncertainties
obtained from scale variation (as described in the text).

As shown in Fig. 5, the distributions are different because of the different
parton shower in Cascade3 and Pythia8, but the ratio of the distributions for
$\rm{Z}+$jet and multijet production are similar: $\rm{Z}+$jet-production
gives a steeper (more strongly correlated) distribution at low $p_{\rm
T}^{\text{leading}}$, while at high $p_{\rm T}^{\text{leading}}$ the
distributions become similar in shape.

The matching scale $\mu_{m}$ limits the hardness of parton-shower emissions,
and is thus typically a non-negligible source of variation in matched
calculations (see e.g. [46] for a detailed discussion). It is thus interesting
to assess the robustness of the previous findings under variations of the
matching scale. Assessing matching scale variations in both an angular-ordered
shower – such as Cascade3 – and a transverse-momentum-ordered shower – such as
Pythia8 – additionally tests the _interpretation_ of the matching scale: in an
angular-ordered shower, the matching scale is applied as “veto scale" to avoid
larger transverse momenta for _any_ emission, while in transverse-momentum
ordered showers, the matching scale sets the maximal transverse momentum of
the _first_ shower emission. The result of changing the matching scale to half
or twice the central value is shown in Fig. 6. As expected, the value of the
matching scale has a impact on the prediction ($\sim 5\%$). This is
particularly apparent when $\mu_{m}$ is used to set the maximal transverse
momentum of the first emission in Pythia8\. Overall, we find that interpreting
the matching scale as veto scale in Cascade3 leads to more robust predictions.
Interestingly, the matching scale uncertainty becomes smaller for
higher-$p_{\rm T}^{\text{leading}}$ jet configurations in Cascade3.

Figure 6: The dependence on the variation of the matching scale $\mu_{m}$ in
predictions for the azimuthal correlation
$\Delta\phi_{\rm{Z}\text{j}}$($\Delta\phi_{12}$) in the back-to-back region.
Shown are predictions obtained with MCatNLO+CAS3 (left column) and
MCatNLO+Pythia8 (right column) for $p_{\rm T}^{\text{leading}}>200$ (upper
row) and $p_{\rm T}^{\text{leading}}>1000$ (lower row). The predictions with
different matching scales $\mu_{m}$ varied by a factor of two up and down are
shown.

In dijet production the measurements are rather well described with
predictions obtained with MCatNLO+CAS3, as shown in Fig. 2 and discussed in
detail in Ref. [14]. Only in the very high $p_{\rm T}^{\text{leading}}$
region, a deviation from the measurement is observed, which could be perhaps
interpreted as coming from a violation of factorization. It is therefore very
important to measure $\Delta\phi$ distributions in other processes, where
factorization is expected to hold.

In order to experimentally probe effects which could originate from
factorization - breaking in the back-to-back region we propose to measure the
ratio of distributions in $\Delta\phi_{\rm{Z}\text{j}}$ for $\rm{Z}+$jet and
$\Delta\phi_{12}$ for multijet production at low and very high $p_{\rm
T}^{\text{leading}}$, and compare the measurement with predictions assuming
that factorization holds. Since the parton configuration of both processes
becomes similar at high $p_{\rm T}^{\text{leading}}$, differences of the ratio
from predictions could hint on possible factorization - breaking effects.

##  4 Summary and conclusions

We have investigated azimuthal correlations in $\rm{Z}+$jet production and
compared predictions with those for multijet production in the same kinematic
range. The predictions are based on PB-TMD distributions with NLO calculations
via MCatNLO supplemented by PB-TMD parton showers via Cascade3. The azimuthal
correlations $\Delta\phi_{\rm{Z}\text{j}}$, obtained in $\rm{Z}+$jet
production are steeper compared to those in multijet production
($\Delta\phi_{12}$) at transverse momenta ${\cal O}(100)$ , while they become
similar for very high transverse momenta, ${\cal O}(1000)$ , which is a result
of similar initial parton configuration of both processes.

In $\rm{Z}+$jet production the color and spin structure of the partonic final
state is different compared to the one in multijet production, and differences
in the azimuthal correlation patterns can be used to search for potential
factorization - breaking effects, involving initial and final state
interferences. In order to experimentally investigate those effects, we
propose to measure the ratio of the distributions in
$\Delta\phi_{\rm{Z}\text{j}}$ for $\rm{Z}+$jet\- and $\Delta\phi_{12}$ for
multijet production at low and at very high $p_{\rm T}^{\text{leading}}$, and
compare the measurements to predictions obtained assuming that factorization
holds.

We have studied the matching scale dependence in the PB-TMD predictions and
compared it with the case of NLO-matched calculations based on the Pythia8
collinear shower. We find that variations of the matching scale lead to more
stable predictions in the PB-TMD case, with the relative reduction of the
matching scale theoretical uncertainty becoming more pronounced for increasing
$p_{\rm T}^{\text{leading}}$ transverse momenta.

Acknowledgments.

We are grateful to Olivier Mattelaer from the MadGraph5_aMC@NLO team for
discussions, help and support with the lhe option for fixed NLO calculations
in MCatNLO.

## References

  * [1] ATLAS Collaboration, “Measurement of dijet azimuthal decorrelations in pp collisions at sqrt(s)=7 TeV”,  Phys.Rev.Lett. 106 (2011) 172002, arXiv:1102.2696.
  * [2] CMS Collaboration, “Dijet Azimuthal Decorrelations in pp Collisions at sqrt(s) = 7 TeV”,  Phys. Rev. Lett. 106 (2011) 122003, arXiv:1101.5029.
  * [3] CMS Collaboration, “Measurement of dijet azimuthal decorrelation in pp collisions at $\sqrt{s}=8\,\mathrm{TeV}$”,  Eur. Phys. J. C 76 (2016) 536, arXiv:1602.04384.
  * [4] CMS Collaboration, “Azimuthal correlations for inclusive 2-jet, 3-jet, and 4-jet events in pp collisions at $\sqrt{s}=$ 13 TeV”,  Eur. Phys. J. C78 (2018) 566, arXiv:1712.05471.
  * [5] CMS Collaboration, “Azimuthal separation in nearly back-to-back jet topologies in inclusive 2- and 3-jet events in pp collisions at $\sqrt{s}=$ 13 TeV”,  Eur. Phys. J. C 79 (2019) 773, arXiv:1902.04374.
  * [6] CDF Collaboration, “Measurement of inclusive jet cross-sections in $Z/\gamma^{*}\to e^{+}e^{-}$ \+ jets production in $p\mathrm{bar}{p}$ collisions at $\sqrt{s}$ = 1.96-TeV”,  Phys. Rev. Lett. 100 (2008) 102001, arXiv:0711.3717.
  * [7] D0 Collaboration, “Measurement of differential $Z/\gamma^{*}$ \+ jet + $X$ cross sections in $p\mathrm{bar}{p}$ collisions at $\sqrt{s}$ = 1.96-TeV”,  Phys. Lett. B669 (2008) 278–286, arXiv:0808.1296.
  * [8] ATLAS Collaboration, “Measurement of the production cross section of jets in association with a Z boson in pp collisions at $\sqrt{s}$ = 7 TeV with the ATLAS detector”,  JHEP 07 (2013) 032, arXiv:1304.7098.
  * [9] ATLAS Collaboration, “Measurement of the production cross section for Z/gamma* in association with jets in $pp$ collisions at $\sqrt{s}=7$ TeV with the ATLAS detector”,  Phys. Rev. D 85 (2012) 032009, arXiv:1111.2690.
  * [10] CMS Collaboration, “Measurements of jet multiplicity and differential production cross sections of $Z+$ jets events in proton-proton collisions at $\sqrt{s}=$ 7 TeV”,  Phys. Rev. D 91 (2015) 052008, arXiv:1408.3104.
  * [11] CMS Collaboration, “Measurements of differential production cross sections for a Z boson in association with jets in pp collisions at $\sqrt{s}=8$ TeV”,  JHEP 04 (2017) 022, arXiv:1611.03844.
  * [12] ATLAS Collaboration, “Measurements of the production cross section of a $Z$ boson in association with jets in pp collisions at $\sqrt{s}=13$ TeV with the ATLAS detector”,  Eur. Phys. J. C 77 (2017) 361, arXiv:1702.05725.
  * [13] CMS Collaboration, “Measurement of differential cross sections for Z boson production in association with jets in proton-proton collisions at $\sqrt{s}=$ 13 TeV”,  Eur. Phys. J. C 78 (2018) 965, arXiv:1804.05252.
  * [14] M. I. Abdulhamid et al., “Azimuthal correlations of high transverse momentum jets at next-to-leading order in the parton branching method”,  Eur. Phys. J. C 82 (2022) 36, arXiv:2112.10465.
  * [15] R. Angeles-Martinez et al., “Transverse Momentum Dependent (TMD) parton distribution functions: status and prospects”,  Acta Phys. Polon. B 46 (2015), no. 12, 2501, arXiv:1507.05267.
  * [16] J. Collins and J.-W. Qiu, “$k_{T}$ factorization is violated in production of high- transverse-momentum particles in hadron-hadron collisions”,  Phys. Rev. D 75 (2007) 114014, arXiv:0705.2141.
  * [17] W. Vogelsang and F. Yuan, “Hadronic Dijet Imbalance and Transverse-Momentum Dependent Parton Distributions”,  Phys. Rev. D 76 (2007) 094013, arXiv:0708.4398.
  * [18] T. C. Rogers and P. J. Mulders, “No Generalized TMD-Factorization in Hadro-Production of High Transverse Momentum Hadrons”,  Phys.Rev. D81 (2010) 094006, arXiv:1001.2977.
  * [19] T. C. Rogers, “Extra spin asymmetries from the breakdown of transverse-momentum-dependent factorization in hadron-hadron collisions”,  Phys. Rev. D 88 (2013), no. 1, 014002, arXiv:1304.4251.
  * [20] F. Hautmann et al., “Soft-gluon resolution scale in QCD evolution equations”,  Phys. Lett. B 772 (2017) 446, arXiv:1704.01757.
  * [21] F. Hautmann et al., “Collinear and TMD quark and gluon densities from Parton Branching solution of QCD evolution equations”,  JHEP 01 (2018) 070, arXiv:1708.03279.
  * [22] J. Alwall et al., “The automated computation of tree-level and next-to-leading order differential cross sections, and their matching to parton shower simulations”,  JHEP 1407 (2014) 079, arXiv:1405.0301.
  * [23] V. N. Gribov and L. N. Lipatov, “Deep inelastic $ep$ scattering in perturbation theory”,  Sov. J. Nucl. Phys. 15 (1972) 438. [Yad. Fiz.15,781(1972)].
  * [24] L. N. Lipatov, “The parton model and perturbation theory”,  Sov. J. Nucl. Phys. 20 (1975) 94. [Yad. Fiz.20,181(1974)].
  * [25] G. Altarelli and G. Parisi, “Asymptotic freedom in parton language”,  Nucl. Phys. B 126 (1977) 298\.
  * [26] Y. L. Dokshitzer, “Calculation of the structure functions for Deep Inelastic Scattering and $e^{+}e^{-}$ annihilation by perturbation theory in Quantum Chromodynamics.”,  Sov. Phys. JETP 46 (1977) 641\. [Zh. Eksp. Teor. Fiz.73,1216(1977)].
  * [27] G. Marchesini and B. R. Webber, “Monte Carlo Simulation of General Hard Processes with Coherent QCD Radiation”,  Nucl. Phys. B310 (1988) 461\.
  * [28] S. Catani, B. R. Webber, and G. Marchesini, “QCD coherent branching and semiinclusive processes at large x”,  Nucl. Phys. B349 (1991) 635–654.
  * [29] F. Hautmann, L. Keersmaekers, A. Lelek, and A. M. Van Kampen, “Dynamical resolution scale in transverse momentum distributions at the LHC”,  Nucl. Phys. B 949 (2019) 114795, arXiv:1908.08524.
  * [30] A. Bermudez Martinez et al., “Collinear and TMD parton densities from fits to precision DIS measurements in the parton branching method”,  Phys. Rev. D 99 (2019) 074008, arXiv:1804.11152.
  * [31] ZEUS, H1 Collaboration, “Combination of measurements of inclusive deep inelastic ${e^{\pm}p}$ scattering cross sections and QCD analysis of HERA data”,  Eur. Phys. J. C 75 (2015) 580, arXiv:1506.06042.
  * [32] A. Bermudez Martinez et al., “Production of Z-bosons in the parton branching method”,  Phys. Rev. D 100 (2019) 074027, arXiv:1906.00919.
  * [33] A. Bermudez Martinez et al., “The transverse momentum spectrum of low mass Drell–Yan production at next-to-leading order in the parton branching method”,  Eur. Phys. J. C 80 (2020), no. 7, 598, arXiv:2001.06488.
  * [34] S. Baranov et al., “CASCADE3 A Monte Carlo event generator based on TMDs”,  Eur. Phys. J. C 81 (2021) 425, arXiv:2101.10221.
  * [35] J. Alwall et al., “A standard format for Les Houches event files”,  Comput. Phys. Commun. 176 (2007) 300, arXiv:hep-ph/0609017.
  * [36] G. Corcella et al., “HERWIG 6.5 release note”, arXiv:hep-ph/0210213.
  * [37] G. Marchesini et al., “HERWIG: A Monte Carlo event generator for simulating hadron emission reactions with interfering gluons. Version 5.1 - April 1991”,  Comput. Phys. Commun. 67 (1992) 465\.
  * [38] M. Cacciari, G. P. Salam, and G. Soyez, “The anti-$k_{t}$ jet clustering algorithm”,  JHEP 04 (2008) 063, arXiv:0802.1189.
  * [39] M. Cacciari, G. P. Salam, and G. Soyez, “FastJet User Manual”,  Eur. Phys. J. C 72 (2012) 1896, arXiv:1111.6097.
  * [40] H. Jung et al., “The CCFM Monte Carlo generator CASCADE version 2.2.03”,  Eur. Phys. J. C 70 (2010) 1237, arXiv:1008.0152.
  * [41] S. Dooling, F. Hautmann, and H. Jung, “Hadroproduction of electroweak gauge boson plus jets and TMD parton density functions”,  Phys. Lett. B736 (2014) 293, arXiv:1406.2994.
  * [42] F. Hautmann and H. Jung, “Angular correlations in multi-jet final states from kt- dependent parton showers”,  JHEP 10 (2008) 113, arXiv:0805.1049.
  * [43] A. Bermudez Martinez, F. Hautmann, and M. L. Mangano, “TMD evolution and multi-jet merging”,  Phys. Lett. B 822 (2021) 136700, arXiv:2107.01224.
  * [44] NNPDF Collaboration, “Parton distributions for the LHC Run II”,  JHEP 04 (2015) 040, arXiv:1410.8849.
  * [45] CMS Collaboration, “Event generator tunes obtained from underlying event and multiparton scattering measurements”,  Eur. Phys. J. C 76 (2016) 155, arXiv:1512.00815.
  * [46] J. Bellm et al., “Parton Shower Uncertainties with Herwig 7: Benchmarks at Leading Order”,  Eur. Phys. J. C 76 (2016) 665, arXiv:1605.01338.


© 2021 IEEE. Personal use of this material is permitted. Permission from IEEE
must be obtained for all other uses, in any current or future media, including
reprinting/republishing this material for advertising or promotional purposes,
creating new collective works, for resale or redistribution to servers or
lists, or reuse of any copyrighted component of this work in other works.

# Efficient solution method based on inverse dynamics for
optimal control problems of rigid body systems

Sotaro Katayama1 and Toshiyuki Ohtsuka1 1S. Katayama and T. Ohtsuka are with
Department of System Science, Graduate School of Informatics, Kyoto
University, Kyoto, Japan<EMAIL_ADDRESS><EMAIL_ADDRESS>
###### Abstract

We propose an efficient way of solving optimal control problems for rigid-body
systems on the basis of inverse dynamics and the multiple-shooting method. We
treat all variables, including the state, acceleration, and control input
torques, as optimization variables and treat the inverse dynamics as an
equality constraint. We eliminate the update of the control input torques from
the linear equation of Newton’s method by applying condensing for inverse
dynamics. The size of the resultant linear equation is the same as that of the
multiple-shooting method based on forward dynamics except for the variables
related to the passive joints and contacts. Compared with the conventional
methods based on forward dynamics, the proposed method reduces the
computational cost of the dynamics and their sensitivities by utilizing the
recursive Newton-Euler algorithm (RNEA) and its partial derivatives. In
addition, it increases the sparsity of the Hessian of the Karush–Kuhn–Tucker
conditions, which reduces the computational cost, e.g., of Riccati recursion.
Numerical experiments show that the proposed method outperforms state-of-the-
art implementations of differential dynamic programming based on forward
dynamics in terms of computational time and numerical robustness.

##  I Introduction

Optimal control plays a significant role in motion planning and control such
as trajectory optimization (TO) and model predictive control (MPC) [1] for
rigid-body systems. TO can generate dynamically consistent motion under
versatile control objectives and constraints even for highly nonlinear systems
such as underactuated systems by solving the optimal control problem (OCP).
MPC leverages the same advantages as TO for real-time control by solving the
OCP at each sampling time. However, we still have to improve the computational
efficiency and numerical robustness of the OCP for rigid-body systems whose
dynamics are complicated and highly nonlinear.

The computational time of the OCP for rigid-body systems depends substantially
on the computational time of the dynamics and their sensitivities. Most
previous researches on the OCP for rigid-body systems (e.g., [2, 3, 4]) have
been based on forward dynamics, which is a calculation of the generalized
acceleration for the given configuration, generalized velocity, and
generalized torques. These studies incorporate forward dynamics into the state
equation, which is a natural representation, especially for single-shooting
methods such as differential dynamic programming (DDP) [5] and the iterative
linear quadratic regulator (iLQR) [6]. They utilize the articulated body
algorithm (ABA) [7], an efficient recursive algorithm to compute forward
dynamics, or directly compute the inverse of the joint inertia matrix, in
solving the OCP. In contrast, our previous work [8] proposed basing the OCP on
inverse dynamics to reduce the computational time compared with that of the
OCP based on forward dynamics. Inverse dynamics are calculations of the
generalized torques for a given configuration, generalized velocity, and
generalized acceleration. We can compute the inverse dynamics with a smaller
computational time than with forward dynamics because the recursive Newton-
Euler algorithm (RNEA) is faster than ABA [9]. Moreover, we can compute the
sensitivities of inverse dynamics faster than those of forward dynamics [10,
11]. Therefore, we can reduce the computational cost of the OCP for rigid-body
systems by using inverse dynamics instead of forward dynamics, as illustrated
numerically in [8]. However, our previous approach of [8] has a potential
drawback when it is utilized for MPC because it is a single-shooting method;
i.e., it regards only the acceleration as the decision variable, which means
it cannot leverage parallel computing and can lack numerical robustness.

The single-shooting method regards only the control input as decision
variables and computes the state on the horizon by simulating the system’s
dynamics based on the control input at each iteration. Typical examples of the
single-shooting method are DDP and iLQR, which solve the linear equation of
Newton’s method by using Riccati recursion [12] and are very popular in
robotics applications [2, 3, 4]. In contrast, the multiple-shooting method
regards all variables (the state, costate, and control inputs) as optimization
variables and can inherently leverage parallel computing by splitting the
computation of the residual of the Karush–Kuhn–Tucker (KKT) conditions and the
Hessian of the KKT condition into each time stage, which is impossible for the
single-shooting methods due to their expensive serial computational parts,
such as the simulation of the system’s dynamics over the horizon. Even in a
single-thread computation, the computational cost of the multiple-shooting
method is almost the same as that of the single-shooting method even though it
has more optimization variables thanks to structure-exploiting Newton-type
methods [13, 14, 12]. Further, the multiple-shooting method is empirically
known to converge quickly even if the initial guess of the solution is far
from the (local) optimal solution; In contrast, the single-shooting method
converges slowly or even diverges in such a situation.

In this paper, we propose an efficient solution method of the OCP for rigid-
body systems based on inverse dynamics and the multiple-shooting method. We
treat all variables, including the state, acceleration, and control input
torques, as optimization variables, and treat the inverse dynamics as an
equality constraint. We eliminate the update of the control input torques from
the linear equation of Newton’s method by applying condensing for inverse
dynamics. The size of the resultant linear equation is the same as that of the
multiple-shooting method based on forward dynamics except for the variables
related to the passive joints and contacts. Compared with the conventional
methods based on forward dynamics, the proposed method reduces the
computational cost of the dynamics and their sensitivities by utilizing RNEA
and its partial derivatives. In addition, it increases the sparsity of the
Hessian of the KKT conditions, which reduces the computational cost, e.g., of
Riccati recursion. Note that the inverse dynamics-based formulation is also
utilized in a contact-implicit TO [15, 16] to alleviate the numerical ill-
conditioning due to an approximation with smooth contact model. In contrast to
these studies, our method is not limited to a specific contact-implicit TO,
e.g., it can treat rigid contacts, as illustrated in a numerical experiment.

This paper is organized as follows. In Section II, we formulate the OCP based
on inverse dynamics. Section III introduces the proposed solution method with
condensing of inverse dynamics. Section IV compares the proposed method with
state-of-the-art implementations of DDP/iLQR and demonstrates its
effectiveness in terms of the computational time and numerical robustness. We
conclude in Section V with a brief summary and mention of future work.

Notation: We describe the partial derivatives of a differentiable function by
certain variables using a function with subscripts; i.e., $f_{x}(x)$ denotes
$\frac{\partial f}{\partial x}(x)$ and $g_{xy}(x,y)$ denotes
$\frac{\partial^{2}g}{\partial x\partial y}(x,y)$. We denote a diagonal matrix
whose elements are a vector $x$ by ${\rm diag}(x)$. Furthermore, we denote an
$n\times n$ identity matrix by $I_{n}$ and an $n\times m$ zero matrix by
$O_{n\times m}$.

##  II Optimal Control Problem Based on Inverse Dynamics

###  II-A Rigid-body systems

Let $Q$ be the configuration manifold of the rigid-body system. Let $q\in Q$,
$v\in\mathbb{R}^{n}$, $a\in\mathbb{R}^{n}$, $f\in\mathbb{R}^{n_{f}}$, and
$u\in\mathbb{R}^{n}$ be the configuration, generalized velocity, acceleration,
stack of the contact forces, and input torques, respectively. The equation of
motion of the rigid-body system is given by

$M(q)a+h(q,v)-J^{\rm T}(q)f=u,$ (1)

where $M(q)\in\mathbb{R}^{n\times n}$ denotes the inertia matrix,
$h(q,v)\in\mathbb{R}^{n}$ encompasses Coriolis, centrifugal, and gravitational
terms, and $J(q)\in\mathbb{R}^{n_{f}\times n}$ denotes the stack of the
contact Jacobians. We write (1) as an equality constraint of the inverse
dynamics as follows:

${\rm ID}(q,v,a,f)-u=0,$ (2)

where ${\rm ID}(q,v,a,f)$ is defined by the left-hand side of (1) in the
formulation of the OCP. Note that we can efficiently compute ${\rm
ID}(q,v,a,f)$ by using RNEA and its partial derivatives by using the partial
derivatives of RNEA [10]. We also assume that the input torques $u$ are an
$n$-dimensional vector even when the system is underactuated, as RNEA and its
partial derivatives are well-defined only for fully actuated systems. As
stated in the next subsection, we treat the underactuated systems by
introducing an equality constraint that zeros the elements of $u$
corresponding to passive joints.

###  II-B Optimal control problem

We consider an OCP with $N$ time stages. The configuration, velocity,
acceleration, external forces, and input torques for $N$ stages are denoted as
$q_{0},...,q_{N}\in Q$, $v_{0},...,v_{N}\in\mathbb{R}^{n}$,
$a_{0},...,a_{N-1}\in\mathbb{R}^{n}$,
$f_{0},...,f_{N-1}\in\mathbb{R}^{n_{f}}$,
$u_{0},...,u_{N-1}\in\mathbb{R}^{n}$, all of which are regarded as
optimization variables to formulate the OCP based on inverse dynamics. We
assume the initial state is given by $\bar{q}$ and $\bar{v}$, and then
consider the constraints on the initial state

$\delta(\bar{q},q_{0})=0,\;\bar{v}-v_{0}=0,\;\;\;\bar{q}\in
Q,\;\bar{v}\in\mathbb{R}^{n},$ (3)

where $\delta(q_{1},q_{2})\in\mathbb{R}^{n}$ denotes the subtraction operation
of the configurations $q_{2}\in Q$ from $q_{1}\in Q$ on the manifold $Q$. The
state equation discretized with the forward Euler method is given by

$\displaystyle\begin{bmatrix}\delta(q_{i},q_{i+1})+v_{i}\Delta\tau\\\
v_{i}-v_{i+1}+a_{i}\Delta\tau\end{bmatrix}=0,\;\;i=0,...,N-1,$ (4)

where $\Delta\tau$ is the time step of the discretization given by
$\Delta\tau=T/N$ with the length of the horizon $T$. The state equation (4)
takes a simple form since we consider the equation of the motion of the system
(1) as an equality constraint (2) in the OCP and regard the acceleration
$a_{i}$ as the optimization variables. In general, we can also assume an
$m_{c}$-dimensional equality constraint,

$C(q_{i},v_{i},a_{i},u_{i},f_{i})\Delta\tau=0,\;\;i=0,...,N-1,$ (5)

and $m_{g}$-dimensional inequality constraint,

$g(q_{i},v_{i},a_{i},u_{i},f_{i})\Delta\tau\leq 0,\;\;i=0,...,N-1.$ (6)

Note that we multiply $C(\cdot)$ and $g(\cdot)$ by $\Delta\tau$ in (5) and (6)
so that the proposed formulation will correspond to the continuous-time Euler-
Lagrange equations discretized with a time step $\Delta\tau$ [17]. If the
system is underactuated, (5) contains the elements of the control input
torques corresponding to the passive joints. For example, if the system has a
floating base whose joint indices are 1 to 6,
$C(q_{i},v_{i},a_{i},u_{i},f_{i})$ in (5) includes
$[u_{i}^{(1)}\,u_{i}^{(2)}\,u_{i}^{(3)}\,u_{i}^{(4)}\,u_{i}^{(5)}\,u_{i}^{(6)}]^{\rm
T}$, where $u_{i}^{(j)}$ is the $j$-th element of $u_{i}$.

For the above description of the rigid-body system and constraints, the OCP is
given by

$\min_{q_{i},v_{i},a_{i},u_{i},f_{i}}J=\varphi(q_{N},v_{N})+\sum_{i=0}^{N-1}l(q_{i},v_{i},a_{i},u_{i},f_{i})\Delta\tau,$

subject to (2)–(6), where $\varphi(q_{N},v_{N})$ denotes the terminal cost and
$l(q_{i},v_{i},a_{i},u_{i},f_{i})\Delta\tau$ denotes the stage cost. We treat
the inequality constraints (6) by using the primal-dual interior point method
(PDIPM) [18], which can treat large-scale and nonlinear constraints
efficiently. The inequality constraint (6) is then transformed into an
equality constraint by introducing slack variables
$s_{i}\in\mathbb{R}^{m_{g}}$,

$g(q_{i},v_{i},a_{i},u_{i},f_{i})\Delta\tau+s_{i}\Delta\tau=0,$ (7)

with an additional inequality constraint $s_{i}\geq 0$.

###  II-C KKT conditions

Next, we derive the KKT conditions, necessary conditions for optimal control
[17, 18]. Let the set of primal variables at stage $i$ be
$y_{i}:=\left\\{q_{i}\;\;v_{i}\;\;a_{i}\;\;f_{i}\;\;u_{i}\right\\}$ and the
set of primal variables without the control input torques be
$\tilde{y}_{i}:=\left\\{q_{i}\;\;v_{i}\;\;a_{i}\;\;f_{i}\right\\}$ for
$i=0,...,N-1$. We then introduce the Lagrangian of this OCP with PDIPM,

$\displaystyle\mathcal{L}=\varphi(q_{N},v_{N})+\sum_{i=0}^{N-1}l(y_{i})\Delta\tau+\begin{bmatrix}\lambda_{0}\\\
\gamma_{0}\end{bmatrix}^{\rm T}\begin{bmatrix}\delta(\bar{q},q_{0})\\\
\bar{v}-v_{0}\end{bmatrix}$
$\displaystyle+\sum_{i=0}^{N-1}\begin{bmatrix}\lambda_{i+1}\\\
\gamma_{i+1}\end{bmatrix}^{\rm
T}\begin{bmatrix}\delta(q_{i},q_{i+1})+v_{i}\Delta\tau\\\
v_{i}-v_{i+1}+a_{i}\Delta\tau\end{bmatrix}$
$\displaystyle+\sum_{i=0}^{N-1}\beta_{i}^{\rm T}({\rm
ID}(\tilde{y}_{i})-u_{i})\Delta\tau+\sum_{i=0}^{N-1}\mu_{i}^{\rm
T}C(y_{i})\Delta\tau$ $\displaystyle+\sum_{i=0}^{N-1}\nu_{i}^{\rm
T}(g(y_{i})\Delta\tau+s_{i}\Delta\tau)-\sum_{i=0}^{N-1}\epsilon\ln
s_{i}\Delta\tau,$

where $\lambda_{i},\;\gamma_{i}\in\mathbb{R}^{n}$,
$\mu_{i}\in\mathbb{R}^{m_{c}}$, and $\nu_{i}\in\mathbb{R}^{m_{g}}$ denote the
Lagrange multipliers with respect to the state equation (4), the equality
constraint (5), and the inequality constraints (6), and $\epsilon>0$ denotes
the barrier parameter. The KKT conditions are then given by (2)–(5), (7),

$\begin{bmatrix}\mathcal{L}_{q_{N}}^{\rm T}\\\ \mathcal{L}_{v_{N}}^{\rm
T}\end{bmatrix}=\begin{bmatrix}\varphi_{q_{N}}^{\rm T}(q_{N},v_{N})\\\
\varphi_{v_{N}}^{\rm
T}(q_{N},v_{N})\end{bmatrix}+\begin{bmatrix}\delta_{q_{N}}^{\rm
T}(q_{N-1},q_{N})\lambda_{N}\\\ -\gamma_{N}\end{bmatrix}=0,$ (8)

and the following equations for $i=0,...,N-1$,

$\displaystyle\begin{bmatrix}\mathcal{L}_{q_{i}}^{\rm T}\\\
\mathcal{L}_{v_{i}}^{\rm T}\end{bmatrix}=$
$\displaystyle\;\begin{bmatrix}l_{q_{i}}^{\rm T}(y_{i})\Delta\tau\\\
l_{v_{i}}^{\rm
T}(y_{i})\Delta\tau\end{bmatrix}+\begin{bmatrix}\delta_{q_{i}}^{\rm
T}(q_{i},q_{i+1})&O_{n\times n}\\\
I_{n}\Delta\tau&I_{n}\end{bmatrix}\begin{bmatrix}\lambda_{i+1}\\\
\gamma_{i+1}\end{bmatrix}$ $\displaystyle+\begin{bmatrix}{\rm ID}_{q_{i}}^{\rm
T}\Delta\tau(\tilde{y}_{i})\\\ {\rm ID}_{v_{i}}^{\rm
T}\Delta\tau(\tilde{y}_{i})\end{bmatrix}\beta_{i}+\begin{bmatrix}{C}_{q_{i}}^{\rm
T}\Delta\tau({y}_{i})\\\ {C}_{v_{i}}^{\rm
T}\Delta\tau({y}_{i})\end{bmatrix}\mu_{i}$
$\displaystyle+\begin{bmatrix}{g}_{q_{i}}^{\rm T}\Delta\tau({y}_{i})\\\
{g}_{v_{i}}^{\rm
T}\Delta\tau({y}_{i})\end{bmatrix}\nu_{i}+\begin{bmatrix}\delta_{q_{i}}^{\rm
T}(q_{i-1},q_{i})\lambda_{i}\\\ -\gamma_{i}\end{bmatrix}=0,$ (9)
$\displaystyle\begin{bmatrix}\mathcal{L}_{a_{i}}^{\rm T}\\\
\mathcal{L}_{f_{i}}^{\rm T}\end{bmatrix}=$
$\displaystyle\;\begin{bmatrix}l_{a_{i}}^{\rm T}(y_{i})\Delta\tau\\\
l_{f_{i}}^{\rm T}(y_{i})\Delta\tau\end{bmatrix}+\begin{bmatrix}O_{n\times
1}\\\ \gamma_{i+1}\Delta\tau\end{bmatrix}+\begin{bmatrix}{\rm ID}_{a_{i}}^{\rm
T}(\tilde{y}_{i})\Delta\tau\\\ {\rm ID}_{f_{i}}^{\rm
T}(\tilde{y}_{i})\Delta\tau\end{bmatrix}\beta_{i}$
$\displaystyle+\begin{bmatrix}{C}_{a_{i}}^{\rm T}({y}_{i})\Delta\tau\\\
{C}_{f_{i}}^{\rm
T}({y}_{i})\Delta\tau\end{bmatrix}\mu_{i}+\begin{bmatrix}{g}_{a_{i}}^{\rm
T}({y}_{i})\Delta\tau\\\ {g}_{f_{i}}^{\rm
T}({y}_{i})\Delta\tau\end{bmatrix}\nu_{i}=0,$ (10) $\mathcal{L}_{u_{i}}^{\rm
T}=l_{u_{i}}^{\rm T}(y_{i})\Delta\tau-\beta_{i}\Delta\tau+C_{u_{i}}^{\rm
T}(y_{i})\mu_{i}\Delta\tau+g_{u_{i}}^{\rm T}(y_{i})\nu_{i}\Delta\tau=0,$ (11)

and

${\rm diag}(s_{i})\nu_{i}=\epsilon 1,$ (12)

where $\epsilon 1$ denotes an $m_{g}$-dimensional vector, all of whose
elements are $\epsilon$. The last equation (12) denotes the complementarity
conditions between the slack variable $s_{i}$ and the Lagrange multiplier
$\nu_{i}$.

##  III Solution Method of Optimal Control Problem

###  III-A Linearization for Newton’s method

The above KKT conditions are linearized for Newton’s method. We apply Gauss-
Newton Hessian approximation because the inverse dynamics constraint (2) is
complicated enough to make it impractical to compute the second-order partial
derivatives of (2). Accordingly, the Hessian of the Lagrangian at the terminal
stage is approximated by the Gauss-Newton-approximated Hessian of the terminal
cost and that at the intermediate stage by the Gauss-Newton-approximated
Hessian of the stage cost. For example, when the terminal cost and the stage
cost take a quadratic form, we have
$\mathcal{L}_{q_{N}q_{N}}\simeq\varphi_{q_{N}q_{N}}$ and
$\mathcal{L}_{q_{i}q_{i}}\simeq l_{q_{i}q_{i}}$. With the approximated
Hessian, (8) is linearized into a linear equation with respect to the Newton
directions $\Delta\lambda_{N},\Delta\gamma_{N},\Delta q_{N},\Delta
v_{N}\in\mathbb{R}^{n}$ as

$\displaystyle\begin{bmatrix}\mathcal{L}_{q_{N}}^{\rm T}\\\
\mathcal{L}_{v_{N}}^{\rm
T}\end{bmatrix}+\begin{bmatrix}\mathcal{L}_{q_{N}q_{N}}&\mathcal{L}_{q_{N}v_{N}}\\\
\mathcal{L}_{v_{N}q_{N}}&\mathcal{L}_{v_{N}v_{N}}\end{bmatrix}\begin{bmatrix}\Delta
q_{N}\\\ \Delta v_{N}\end{bmatrix}$
$\displaystyle+\begin{bmatrix}\delta_{q_{N}}^{\rm
T}(q_{N-1},q_{N})\Delta\lambda_{N}\\\ -\Delta\gamma_{N}\end{bmatrix}=0,$ (13)

As well, (II-C)–(11) are linearized into linear equations with respect to the
Newton directions
$\Delta\lambda_{i+1},\allowbreak\Delta\gamma_{i+1},\allowbreak\Delta
q_{i},\allowbreak\Delta v_{i},\allowbreak\Delta a_{i},\allowbreak\Delta
u_{i},\Delta\beta_{i}\in\mathbb{R}^{n}$, $\Delta f_{i}\in\mathbb{R}^{n_{f}}$,
and $\Delta\mu_{i}\allowbreak\in\mathbb{R}^{m_{c}}$. Note that the directions
related to the PDIPM, $\Delta s_{i},\Delta\nu_{i}\in\mathbb{R}^{m_{g}}$, are
eliminated explicitly from the linear equations by adding certain terms
related to the logarithmic barrier functions to the Hessians (e.g.,
${\mathcal{L}}_{q_{i}q_{i}}$) and residuals of the KKT conditions (e.g.,
${\mathcal{L}}_{q_{i}}$) [19, 18]. By denoting such modified Hessians as
$\bar{\mathcal{L}}_{q_{i}q_{i}}$ and KKT residuals as
$\bar{\mathcal{L}}_{q_{i}}$, we obtain

$\displaystyle\begin{bmatrix}\bar{\mathcal{L}}_{q_{i}}^{\rm T}\\\
\bar{\mathcal{L}}_{v_{i}}^{\rm
T}\end{bmatrix}+\begin{bmatrix}\bar{\mathcal{L}}_{q_{i},y_{i}}\\\
\bar{\mathcal{L}}_{v_{i},y_{i}}\end{bmatrix}\Delta
y_{i}+\begin{bmatrix}\delta_{q_{i}}^{\rm T}(q_{i},q_{i+1})&O_{n\times n}\\\
I_{n}\Delta\tau&I_{n}\end{bmatrix}\begin{bmatrix}\Delta\lambda_{i+1}\\\
\Delta\gamma_{i+1}\end{bmatrix}$ $\displaystyle+\begin{bmatrix}{\rm
ID}_{q_{i}}^{\rm T}(\tilde{y}_{i})\Delta\tau\\\ {\rm ID}_{v_{i}}^{\rm
T}(\tilde{y}_{i})\Delta\tau\end{bmatrix}\Delta\beta_{i}+\begin{bmatrix}{C}_{q_{i}}^{\rm
T}({y}_{i})\Delta\tau\\\ {C}_{v_{i}}^{\rm
T}({y}_{i})\Delta\tau\end{bmatrix}\Delta\mu_{i}$
$\displaystyle+\begin{bmatrix}\delta_{q_{i}}^{\rm
T}(q_{i-1},q_{i})\Delta\lambda_{i}\\\ -\Delta\gamma_{i}\end{bmatrix}=0,$ (14)
$\displaystyle\begin{bmatrix}\bar{\mathcal{L}}_{a_{i}}^{\rm T}\\\
\bar{\mathcal{L}}_{f_{i}}^{\rm
T}\end{bmatrix}+\begin{bmatrix}\bar{\mathcal{L}}_{a_{i}y_{i}}\\\
\bar{\mathcal{L}}_{f_{i}y_{i}}\end{bmatrix}\Delta
y_{i}+\begin{bmatrix}O_{n\times 1}\\\
\Delta\gamma_{i+1}\Delta\tau\end{bmatrix}$ $\displaystyle+\begin{bmatrix}{\rm
ID}_{a_{i}}^{\rm T}(\tilde{y}_{i})\Delta\tau\\\ {\rm ID}_{f_{i}}^{\rm
T}(\tilde{y}_{i})\Delta\tau\end{bmatrix}\Delta\beta_{i}+\begin{bmatrix}{C}_{a_{i}}^{\rm
T}({y}_{i})\Delta\tau\\\ {C}_{f_{i}}^{\rm
T}({y}_{i})\Delta\tau\end{bmatrix}\Delta\mu_{i}=0,$ (15)

and

$\bar{\mathcal{L}}_{u_{i}}^{\rm T}+\bar{\mathcal{L}}_{u_{i}y_{i}}\Delta
y_{i}-\Delta\beta_{i}\Delta\tau+C_{u_{i}}^{\rm
T}(y_{i})\Delta\mu_{i}\Delta\tau=0.$ (16)

The constraints, (2)–(5) and (7), are linearized as

$\delta(\bar{q},q_{0})+\delta_{q_{0}}(\bar{q},q_{0})\Delta
q_{0}=0,\;\bar{v}-v_{0}-\Delta v_{0}=0,$ (17)
$\displaystyle\begin{bmatrix}\delta(q_{i},q_{i+1})+v_{i}\Delta\tau\\\
v_{i}-v_{i+1}+a_{i}\Delta\tau\end{bmatrix}+\begin{bmatrix}\delta_{q_{i}}(q_{i},q_{i+1})&I_{n}\Delta\tau\\\
O_{n\times n}&I_{n}\end{bmatrix}\begin{bmatrix}\Delta{q_{i}}\\\
\Delta{v_{i}}\end{bmatrix}$ $\displaystyle\;\;+\begin{bmatrix}O_{n\times 1}\\\
\Delta
a_{i}\Delta\tau\end{bmatrix}+\begin{bmatrix}\delta_{q_{i+1}}(q_{i},q_{i+1})&O_{n\times
n}\\\ O_{n\times n}&-I_{n}\end{bmatrix}\begin{bmatrix}\Delta{q_{i+1}}\\\
\Delta{v_{i+1}}\end{bmatrix}=0,$ (18) ${\rm
ID}_{\tilde{y}_{i}}(\tilde{y})\Delta\tilde{y}_{i}-\Delta u_{i}+{\rm
ID}(\tilde{y})-u=0,$ (19)

and

${C}_{\tilde{y}_{i}}(y_{i})\Delta\tilde{y}_{i}+{C}_{u}(y_{i})\Delta
u_{i}+C(y_{i})=0.$ (20)

Each Newton iteration consists of solving a linear equation that finds Newton
directions satisfying (III-A)–(20).

###  III-B Condensing inverse dynamics

Next, we condense the inverse dynamics; i.e., we eliminate $\Delta u_{i}$ and
$\Delta\beta_{i}$ from the linear equations (III-A)–(20). By substituting the
expression of $\Delta u_{i}$ and $\Delta\beta_{i}$ with respect to other
Newton directions (19) and (16) into (III-A)–(16) and (20), we can obtain a
condensed linear equation. For notational simplicity, we introduce the
condensed Hessian,

$\displaystyle\tilde{\mathcal{L}}_{z_{i}w_{i}}:=\;$
$\displaystyle\bar{\mathcal{L}}_{z_{i}w_{i}}+{\rm ID}_{z_{i}}^{\rm
T}(\tilde{y}_{i})\bar{\mathcal{L}}_{u_{i}u_{i}}{\rm
ID}_{w_{i}}(\tilde{y}_{i})$ $\displaystyle+{\rm ID}_{z_{i}}^{\rm
T}(\tilde{y})\bar{\mathcal{L}}_{u_{i}w_{i}}+\bar{\mathcal{L}}_{z_{i}u_{i}}{\rm
ID}_{w_{i}}(\tilde{y})$ (21)

for $z_{i},w_{i}\in\left\\{q_{i},v_{i},a_{i},f_{i}\right\\}$ and the condensed
KKT residual,

$\displaystyle{\tilde{\mathcal{L}}_{z_{i}}}^{\rm T}:=\;$
$\displaystyle{\bar{\mathcal{L}}_{z_{i}}}^{\rm T}+{\rm ID}_{z_{i}}^{\rm
T}(\tilde{y}){\bar{\mathcal{L}}_{u_{i}}}^{\rm T}$
$\displaystyle+(\bar{\mathcal{L}}_{z_{i},u_{i}}+{\rm ID}_{z_{i}}^{\rm
T}(\tilde{y})\bar{\mathcal{L}}_{u_{i}u_{i}})({\rm ID}(\tilde{y})-u_{i})$ (22)

for $z_{i}\in\left\\{q_{i},v_{i},a_{i},f_{i}\right\\}$. We also introduce the
condensed Jacobian of the equality constraint,

$\displaystyle\tilde{C}_{z_{i}}:={C}_{z_{i}}+C_{u_{i}}{\rm
ID}_{z_{i}}(q_{i},v_{i},a_{i},f_{i}),$ (23)

for $z_{i}\in\left\\{q_{i},v_{i},a_{i},f_{i}\right\\}$ and the condensed
residual of the equality constraint,

$\displaystyle\tilde{C}:={C}+C_{u_{i}}({\rm
ID}(q_{i},v_{i},a_{i},f_{i})-u_{i}).$ (24)

Then, $\Delta u_{i}$ and $\Delta\beta_{i}$ are eliminated from (III-A)–(16)
and (20):

$\displaystyle\begin{bmatrix}\tilde{\mathcal{L}}_{q_{i}}^{\rm T}\\\
\tilde{\mathcal{L}}_{v_{i}}^{\rm
T}\end{bmatrix}+\begin{bmatrix}\tilde{\mathcal{L}}_{q_{i}y_{i}}\\\
\tilde{\mathcal{L}}_{v_{i}y_{i}}\end{bmatrix}\Delta
y_{i}+\begin{bmatrix}\delta_{q_{i}}^{\rm T}(q_{i},q_{i+1})&O_{n\times n}\\\
I_{n}\Delta\tau&I_{n}\end{bmatrix}\begin{bmatrix}\Delta\lambda_{i+1}\\\
\Delta\gamma_{i+1}\end{bmatrix}$
$\displaystyle+\begin{bmatrix}{C}_{q_{i}}^{\rm T}({y}_{i})\\\ {C}_{v_{i}}^{\rm
T}({y}_{i})\end{bmatrix}\Delta\mu_{i}+\begin{bmatrix}\delta_{q_{i}}^{\rm
T}(q_{i-1},q_{i})\Delta\lambda_{i}\\\ -\Delta\gamma_{i}\end{bmatrix}=0,$ (25)
$\begin{bmatrix}\tilde{\mathcal{L}}_{a_{i}}^{\rm T}\\\
\tilde{\mathcal{L}}_{f_{i}}^{\rm
T}\end{bmatrix}+\begin{bmatrix}\tilde{\mathcal{L}}_{a_{i}y_{i}}\\\
\tilde{\mathcal{L}}_{f_{i}y_{i}}\end{bmatrix}\Delta
y_{i}+\begin{bmatrix}O_{n\times 1}\\\
\Delta\gamma_{i+1}\Delta\tau\end{bmatrix}+\begin{bmatrix}{C}_{a_{i}}^{\rm
T}({y}_{i})\\\ {C}_{f_{i}}^{\rm T}({y}_{i})\end{bmatrix}\Delta\mu_{i}=0,$ (26)

and

$\tilde{C}_{\tilde{y}_{i}}(y_{i})\Delta\tilde{y}_{i}+\tilde{C}(y_{i})=0.$ (27)

After condensing, the linear equation is reduced to find the Newton directions
$\Delta\lambda_{i},\Delta\gamma_{i},\Delta q_{i},\Delta v_{i},\Delta
a_{i},\Delta f_{i},\Delta\mu_{i}$ satisfying (17), (III-A), (III-A),
(III-B)–(27).

If the rigid-body system is fully actuated and there are no contacts, the size
of the condensed linear equation is the same as that of the multiple-shooting
method based on forward dynamics. If it is underactuated and there are no
contacts, the size of the condensed linear equation at each time stage is
increased from that of the multiple-shooting method based on forward dynamics
by only twice the number of the passive joints, since we assume the system is
fully actuated and add an equality constraint to zero the control input
torques corresponding to the passive joints. If there are contacts, the size
of the linear equation also increases depending on the way to treat the
contacts.

###  III-C Algorithm

Algorithm 1 is the pseudocode of a single Newton iteration of the proposed
method. The first step (lines 1–3) forms the linear equation consisting of
(17), (III-A), (III-A), and (III-B)–(27) by computing the condensed KKT
residuals and Hessians, such as $\tilde{\mathcal{L}}_{q_{i}}$,
$\tilde{\mathcal{L}}_{q_{i}q_{i}}$, $\tilde{C}$, and $\tilde{C}_{q}$. This
step is fully parallelizable into each time stage. The second step (line 4–6)
computes the directions $\Delta\lambda_{i}$, $\Delta\gamma_{i}$,
$\Delta\tilde{y}_{i}$, $\Delta\mu_{i}$ by solving the linear equation
consisting of (17), (III-A), (III-A), and (III-B)–(27), i.e., inverting the
condensed Hessian. In this step, we can utilize various efficient methods
tailored for the OCP [20], e.g., Riccati recursion [12] and a highly
parallelizable method [21]. The third step (lines 7–9) computes the condensed
directions, i.e., $\Delta u_{i}$ and $\Delta\beta_{i}$ from (19) and (16), and
$\Delta s_{i}$ and $\Delta\nu_{i}$ according to the PDIPM [19, 18]. The fourth
step (line 10) determines the step size, e.g., by using the fraction-to-
boundary rule [18, 19]. Finally, all variables are updated according to the
step size (lines 11–16).

Algorithm 1 Single Newton iteration of primal-dual interior point method with
condensing of inverse dynamics

1:Initial state $\bar{q},\bar{v}$

2:$\lambda_{0},...,\lambda_{N}$, $\gamma_{0},...,\gamma_{N}$,
$y_{0},...,y_{N-1}$, $q_{N}$, $v_{N}$, $\mu_{0},...,\mu_{N-1}$,
$\beta_{0},...,\beta_{N-1}$, $s_{0},...,s_{N-1}$, $\nu_{0},...,\nu_{N-1}$

3:for $i=0,\cdots,N$ do in parallel

4: Compute the condensed Hessian and KKT residual.

5:end for

6:for $i=0,\cdots,N$ do in parallel or serial

7: Compute the Newton directions $\Delta\lambda_{i}$, $\Delta\gamma_{i}$,
$\Delta\tilde{y}_{i}$, $\Delta\mu_{i}$ by solving the linear equation (17),
(III-A), (III-A), (III-B)–(27).

8:end for

9:for $i=0,\cdots,N-1$ do in parallel

10: Compute the condensed Newton directions $\Delta u_{i}$ and
$\Delta\beta_{i}$ from (19), (16), and $\Delta s_{i}$ and $\Delta\nu_{i}$
according to [19].

11:end for

12:Determine the step size $\alpha\in(0,1]$.

13:for $i=0,\cdots,N$ do in parallel

14: Update all variables $\lambda_{i}$, $\gamma_{i}$, $y_{i}$, $\mu_{i}$
$\beta_{i}$, $s_{i}$, $\nu_{i}$ by

15: $\lambda_{i}\leftarrow\lambda_{i}+\alpha\Delta\lambda_{i}$,
$\gamma_{i}\leftarrow\gamma_{i}+\alpha\Delta\gamma_{i}$, $y_{i}\leftarrow
y_{i}+\alpha\Delta y_{i}$,

16: $\mu_{i}\leftarrow\mu_{i}+\alpha\Delta\mu_{i}$,
$\beta_{i}\leftarrow\beta_{i}+\alpha\Delta\beta_{i}$,

17: $s_{i}\leftarrow s_{i}+\alpha\Delta s_{i}$,
$\nu_{i}\leftarrow\nu_{i}+\alpha\Delta\nu_{i}$

18:end for

The main advantage of the proposed method appears in the first step (lines
1–3). It calls RNEA and the partial derivatives of RNEA $N-1$ times, whereas
the forward dynamics-based method computes ABA (or inverse of the joint-
inertia matrix) $N-1$ times and the partial derivatives of ABA $N-1$ times.
Moreover, the proposed method can reduce the computational cost in the second
step (lines 4–6), as our formulation leads to sparsity in the partial
derivatives of the state equation. We will explain this point below with the
Riccati recursion for computing the Newton directions. Riccati recursion [12]
performs a recursive block elimination to compute the Newton directions with
the forward Euler discretization (4). In Riccati recursion, we regard $[\Delta
q_{i}^{\rm T}\;\;\Delta v_{i}^{\rm T}]^{\rm T}$ as the state variable and
$[\Delta a_{i}^{\rm T}\;\;\Delta f_{i}^{\rm T}]^{\rm T}$ as the control input
of a linear quadratic regulator subproblem. We also introduce matrices,

$A_{i}:=\begin{bmatrix}\delta_{q_{i}}(q_{i},q_{i+1})&I_{n}\Delta\tau\\\
O_{n\times n}&I_{n}\end{bmatrix},\;\;B_{i}:=\begin{bmatrix}O_{n\times
n}&O_{n\times n_{f}}\\\ \Delta\tau I_{n}&O_{n\times n_{f}}\end{bmatrix},$

where $A_{i}$ is composed of the partial derivatives of the state equation
with respect to $q$ and $v$, and $B_{i}$ is composed of the partial
derivatives with respect to $a$ and $f$. Riccati recursion serially computes
$A_{i}^{\rm T}P_{i+1}A_{i}$, $A_{i}^{\rm T}P_{i+1}B_{i}$, $B_{i}^{\rm
T}P_{i+1}B_{i}$ for a sequence of matrices $P_{i}$ that represent the
sensitivities of $\Delta\lambda_{i}$ and $\Delta\gamma_{i}$ with respect to
$\Delta q_{i}$ and $\Delta v_{i}$. Thanks to the sparsity of $A_{i}$ and
$B_{i}$ in the proposed formulation, several matrix products reduce to sums of
matrices, and this in turn reduces the computational cost, especially the cost
of the serial computational part.

We have developed an open-source C++ software framework for the OCP for rigid-
body systems, idocp [22], that utilizes the above Riccati recursion and
ParNMPC, a highly parallelizable method [21]. idocp uses
Eigen111http://eigen.tuxfamily.org for linear algebra and pinocchio [23], an
efficient C++ library for rigid-body dynamics algorithms, to compute RNEA and
its partial derivatives. It also employs the parallel Newton-type method [21]
with backward Euler discretization method.

##  IV Numerical Experiments

###  IV-A Experimental settings

To evaluate the computational efficiency and numerical robustness of the
proposed solution method of the OCP, we compared our solver idocp with
crocoddyl [4], a highly efficient C++ implementation of DDP/iLQR for rigid-
body systems, and Ipopt [19], an off-the-shelf nonlinear optimization solver.
Both idocp and crocoddyl utilize pinocchio for rigid-body dynamics algorithms,
Eigen for linear algebra, and OpenMP [24] for parallel computing. Ipopt was
customized to solve the OCP for rigid-body systems based on forward dynamics
and the multiple-shooting method with parallel computing by using pinocchio
and OpenMP. As the algorithm of idocp, we used Riccati recursion to compute
the Newton directions (we will refer to this method as inverse dynamics-based
Riccati recursion (IDRR) hereafter). Furthermore, we utilized two algorithms
from crocoddyl: the standard DDP with a Gauss-Newton Hessian approximation,
which is identical to iLQR, and feasible-prone DDP (FDDP) [4], a kind of iLQR
that improves numerical robustness by modifying the backward pass of iLQR in a
multiple-shooting fashion [25] and utilizes a line-search method based on the
Goldstein condition [18]. We will refer to the former as iLQR and the latter
as FDDP. In Ipopt, we used the Broyden–Fletcher–Goldfarb–Shanno method for the
Hessian approximation and Harwell Subroutine Library MA57 to solve the linear
subproblems. All experiments were conducted on a laptop with quad-core CPU
Intel Core i7-10510U @1.8GHz.

###  IV-B Computational time

Figure 1: Average computational time per iteration ${\rm[ms]}$ of the proposed
method (${\rm IDRR}$), ${\rm iLQR}$/${\rm FDDP}$, and Ipopt, for various
degrees of freedom (DOF $\in\left\\{7,14,32\right\\}$), numbers of time steps
($N\in\left\\{50,100\right\\}$), and numbers of threads (${\rm
nproc}\in\left\\{1,4\right\\}$)

First, we compared the computational time per iteration of IDRR, iLQR/FDDP,
and Ipopt. In particular, we computed the average computational time over
10,000 trials for systems with various degrees of freedom (DOF), various
numbers of time stages $N$, and various numbers of threads (nproc) with the
quadratic terminal cost,

$\varphi(q,v)=\frac{1}{2}q_{e}^{\rm T}Q_{q}q_{e}+\frac{1}{2}v_{e}^{\rm
T}Q_{v}v_{e},$ (28)

where $Q_{q}=I_{n}$, $Q_{v}=I_{n}$, $q_{e}:=q-q_{\rm ref}$, and
$v_{e}:=v-v_{\rm ref}$ with $q_{\rm ref},v_{\rm ref}\in\mathbb{R}^{n}$, and
the quadratic stage cost,

$l(q,v,a,u)=\frac{1}{2}q_{e}^{\rm T}Q_{q}q_{e}+\frac{1}{2}v_{e}^{\rm
T}Q_{v}v_{e}+\frac{1}{2}u_{e}^{\rm T}Q_{u}u_{e},$ (29)

where $Q_{u}=0.001\times I_{n}$ and $u_{e}:=u-u_{\rm ref}$ with $u_{\rm
ref}\in\mathbb{R}^{n}$. Fig. 1 shows the computational time per iteration [ms]
of each solver. Note that the results of iLQR and FDDP are shown as one
because they had almost the same computational time. Moreover, the results of
Ipopt are only for the fastest case, i.e., the case with $N=50$ and nproc = 4,
because Ipopt was much slower than the other solvers. As shown in the figure,
for all combinations of $N$ and number of threads (nproc), our IDRR was faster
than iLQR/FDDP and Ipopt. This demonstrates that the inverse dynamics-based
formulation can reduce the computational cost compared with the forward
dynamics-based formulation. We also found that IDRR became faster as the
number of the threads increased, whereas iLQR/FDDP did so only moderately.
This is because the proposed method reduces the computational burden of the
serial calculation in the Riccati recursion, thanks to its multiple-shooting
and sparsity structure, as stated in III-C.

###  IV-C Numerical robustness

Figure 2:  $\log_{10}$ scaled KKT errors of the proposed method (IDRR), FDDP,
and iLQR over 20 random trials.

Second, we investigated the numerical robustness, i.e., the convergence, of
the proposed method through the OCP for KUKA iiwa14, a 7-DOF manipulator. We
set the length of the horizon to 1 s and divided it into $N=50$ steps, i.e.,
$\Delta\tau=0.02{\rm[s]}$. The objective of the OCP is to make the
configuration $q$ converge to $q_{\rm ref}$ and the velocity $v$ converge to
zero given a random initial state $\bar{q}$ and $\bar{v}$. We used the
quadratic terminal cost (28) and stage cost (29) and set $q_{\rm ref}$ to
$[0\;\;\pi/2\;\;0\;\;\pi/2\;\;0\;\;\pi/2\;\;0]^{\rm T}$, $v_{\rm ref}$ to
zero, and $u_{\rm ref}$ to the gravity compensation torques at $q_{\rm ref}$.
We did not impose any inequality constraints in this experiment. iLQR and FDDP
used a line search and did not use regularization. IDRR did not use either,
while Ipopt used both of them. We initialized $q_{i}$ and $v_{i}$ in the
solution of IDRR, FDDP, and Ipopt by $\bar{q}$ and $\bar{v}$. We randomly
selected each element of $\bar{q}$ from $[-1,1]$ and each element of $\bar{v}$
from $[-10,10]$. We ran 20 trials for each solver and computed the
$l^{2}$-norm of the residual of the KKT conditions, which we will refer to as
the KKT error hereafter, and the total cost to be minimized. Note that the KKT
conditions of IDRR are given by (2)–(5), (7), and (8)–(12), while those of
iLQR/FDDP and Ipopt are composed of different equations based on forward
dynamics and/or single-shooting. Figure 2 shows the $\log_{10}$-scaled KKT
errors of IDRR, FDDP, and iLQR. Results for Ipopt are not shown because it
converged rather slowly (over 1000 iterations were needed to reduce the KKT
error to under $10^{-10}$ in most cases) due to the quasi-Newton Hessian
approximation. Note as well that, as the equations representing the KKT
conditions differ between IDRR and FDDP/iLQR, we cannot directly compare their
KKT errors. However, from the graph of FDDP, we can clearly see that FDDP
diverged in two trials. In addition, from the graph of iLQR, we can see that
in many trials the KKT error could not be completely reduced because the iLQR
did not produce a descent direction and the step size became zero. On the
other hand, IDRR reduced the KKT error at each iteration in all cases,
including those in which FDDP and iLQR failed to converge. This indicates that
the proposed method is more robust than single-shooting methods such as iLQR
and FDDP.

###  IV-D MPC for floating base systems

Finally, we investigated the applicability of the proposed method to floating
base systems, which are a kind of underactuated system, by simulating MPC
implemented by IDRR for the whole-body control of a quadruped ANYmal. To take
the rigid contacts into account, we treated the contact forces as the
optimization variables and imposed an equality constraint in the form of
Baumgarte’s stabilization method [26] for each contact, i.e., a 12-dimensional
equality constraint for four contacts. We used a quadratic cost function to
track the desired configuration. We also imposed inequality constraints on the
joint angle limits, angular velocity limits, and torque limits and linearized
friction cones using the PDIPM. We set the length of the horizon to 1 s and
divided it into $N=20$ equal steps. We set the sampling time to 2.5 ms, and
the MPC controller updated the solution once per sampling period. We utilized
RaiSim [27], an articulated-body simulator with contacts. Figure 3 shows the
various postures of ANYmal controlled by MPC. As can be seen, the proposed
method was able to control the underactuated system. Each control update took
around 2.1 ms with four threads on the same laptop as in IV-B and achieved
real-time MPC.

Figure 3: Posture control of ANYmal by whole-body MPC controller.

##  V Conclusions

We proposed an efficient method of solving the OCP for rigid-body systems on
the basis of inverse dynamics and the multiple-shooting method. In this
method, we regard all variables, including the state, acceleration, and
control input torques, as optimization variables and treat the inverse
dynamics as an equality constraint. We eliminated the update in the control
input torques from the linear equation of Newton’s method by applying
condensing for inverse dynamics. The size of the resultant linear equation was
the same as that of the multiple-shooting method based on forward dynamics
except for the variables related to the passive joints and contacts. The
proposed method reduces the computational cost of the dynamics and their
sensitivities by utilizing RNEA and its partial derivatives. In addition, it
increases the sparsity of the Hessian of the KKT conditions, which further
reduces the computational cost, e.g., of Riccati recursion. Numerical
experiments showed that the proposed method is more than twice as fast as iLQR
and a DDP variant based on forward dynamics. They also showed that it is more
numerically robust than these conventional methods.

Our future work will include incorporating changes in contact status by means
of the hybrid optimal control approach [28] or the contact-implicit approach
[16, 29] with an eye toward developing applications for manipulation and
locomotion. Both methods can be combined with the proposed method because the
contact forces are included in the optimization variables.

## References

  * [1] L. Magni, D. Raimondo, and F. Allgöwer, _Nonlinear Model Predictive Control: Towards New Challenging Applications_. Springer, 2008.
  * [2] J. Koenemann, A. Del Prete, Y. Tassa, E. Todorov, O. Stasse, M. Bennewitz, and N. Mansard, “Whole-body model-predictive control applied to the HRP-2 humanoid,” in _2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , 2015, pp. 3346–3351.
  * [3] M. Neunert, M. Stäuble, M. Giftthaler, C. D. Bellicoso, J. Carius, C. Gehring, M. Hutter, and J. Buchli, “Whole-body nonlinear model predictive control through contacts for quadrupeds,” _IEEE Robotics and Automation Letters_ , vol. 3, no. 3, pp. 1458–1465, 2018.
  * [4] C. Mastalli, R. Budhiraja, W. Merkt, G. Saurel, B. Hammoud, M. Naveau, J. Carpentier, L. Righetti, S. Vijayakumar, and N. Mansard, “Crocoddyl: An efficient and versatile framework for multi-contact optimal control,” in _2020 IEEE International Conference on Robotics and Automation (ICRA)_ , 2020, pp. 2536–2542.
  * [5] Y. Tassa, T. Erez, and E. Todorov, “Synthesis and stabilization of complex behaviors through online trajectory optimization,” in _2012 IEEE/RSJ International Conference on Intelligent Robots and Systems_ , 2012, pp. 4906–4913.
  * [6] E. Todorov and W. Li, “A generalized iterative LQG method for locally-optimal feedback control of constrained nonlinear stochastic systems,” in _Proceedings of the 2005, American Control Conference_ , 2005, pp. 300–306.
  * [7] R. Featherstone, “The calculation of robot dynamics using articulated-body inertias,” _The International Journal of Robotics Research_ , vol. 2, no. 1, pp. 13–30, 1983.
  * [8] S. Katayama and T. Ohtsuka, “Efficient solution method based on inverse dynamics of optimal control problems for fixed-based rigid-body systems,” in _2020 IFAC World Congress_ , 2020, pp. 363–368.
  * [9] R. Featherstone, _Rigid Body Dynamics Algorithms_. Springer, 2008.
  * [10] J. Carpentier and N. Mansard, “Analytical derivatives of rigid body dynamics algorithms,” in _Robotics: Science and Systems (RSS 2018)_ , 2018, pp. hal–01 790 971v2f.
  * [11] M. Neunert, M. Giftthaler, M. Frigerio, C. Semini, and J. Buchli, “Fast derivatives of rigid body dynamics for control, optimization and estimation,” in _2016 IEEE International Conference on Simulation, Modeling, and Programming for Autonomous Robots (SIMPAR)_ , 2016, pp. 91–97.
  * [12] G. Frison, “Algorithms and methods for high-performance model predictive control,” Ph.D. dissertation, Technical University of Denmark, 2016.
  * [13] H. Bock and K. Plitt, “A multiple shooting algorithm for direct solution of optimal control problems,” in _9th IFAC World Congress_ , 1984, pp. 1603–1608.
  * [14] J. Albersmeyer and M. Diehl, “The lifted Newton method and its application in optimization,” _SIAM Journal on Optimization_ , vol. 20, no. 3, pp. 1655–1684, 2010.
  * [15] T. Erez and E. Todorov, “Trajectory optimization for domains with contacts using inverse dynamics,” in _2012 IEEE/RSJ International Conference on Intelligent Robots and Systems_ , 2012, pp. 4914–4919.
  * [16] E. Todorov, “Convex and analytically-invertible dynamics with contacts and constraints: Theory and implementation in mujoco,” in _2014 IEEE International Conference on Robotics and Automation (ICRA)_ , 2014, pp. 6054–6061.
  * [17] A. E. Bryson and Y.-C. Ho, _Applied Optimal Control: Optimization, Estimation, and Control_. CRC Press, 1975\.
  * [18] J. Nocedal and S. J. Wright, _Numerical Optimization_ , 2nd ed. Springer, 2006.
  * [19] A. Wächter and L. Biegler, “On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming,” _Mathematical Programming_ , vol. 106, pp. 25–57, 2006.
  * [20] A. Z. D. Kouzoupis, G. Frison and M. Diehl, “Recent advances in quadratic programming algorithms for nonlinear model predictive control,” _Vietnam Journal of Mathematics_ , vol. 46, no. 4, pp. 863–882, 2018.
  * [21] H. Deng and T. Ohtsuka, “A parallel Newton-type method for nonlinear model predictive control,” _Automatica_ , vol. 109, p. 108560, 2019.
  * [22] S. Katayama, “idocp,” 2020. [Online]. Available: https://github.com/mayataka/idocp
  * [23] J. Carpentier, G. Saurel, G. Buondonno, J. Mirabel, F. Lamiraux, O. Stasse, and N. Mansard, “The Pinocchio C++ library – A fast and flexible implementation of rigid body dynamics algorithms and their analytical derivatives,” in _International Symposium on System Integration (SII)_ , 2019, pp. 614 – 619.
  * [24] L. Dagum and R. Menon, “OpenMP: An industry-standard API for shared-memory programming,” _IEEE Computational Science & Engineering_, vol. 5, no. 1, p. 46–55, 1998.
  * [25] M. Giftthaler, M. Neunert, M. Stäuble, J. Buchli, and M. Diehl, “A family of iterative Gauss-Newton shooting methods for nonlinear optimal control,” in _2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , 2018, pp. 1–9.
  * [26] J. Baumgarte, “Stabilization of constraints and integrals of motion in dynamical systems,” _Computer Methods in Applied Mechanics and Engineering_ , vol. 1, no. 1, pp. 1–16, 1972.
  * [27] J. Hwangbo, J. Lee, and M. Hutter, “Per-contact iteration method for solving contact dynamics,” _IEEE Robotics and Automation Letters_ , vol. 3, no. 2, pp. 895–902, 2018.
  * [28] S. Katayama, M. Doi, and T. Ohtsuka, “A moving switching sequence approach for nonlinear model predictive control of switched systems with state-dependent switches and state jumps,” _International Journal of Robust and Nonlinear Control_ , vol. 30, no. 2, pp. 719–740, 2020.
  * [29] M. Posa, C. Cantu, and R. Tedrake, “A direct method for trajectory optimization of rigid bodies through contact,” _The International Journal of Robotics Research_ , vol. 33, no. 1, pp. 69–81, 2014.