{'fraction_non_alphanumeric': 0.040676269793044284, 'fraction_numerical': 0.011152017728848697, 'mean_word_length': 4.605690242436386, 'pattern_counts': {'":': 0, '<': 0, '<?xml version=': 0, '>': 3, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 1, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
Implicit Integration for Articulated Bodies with Contact via the Nonconvex Maximal Dissipation Principle

We present non-convex maximal dissipation principle (NMDP), a time integration scheme for articulated bodies with simultaneous contacts. Our scheme resolves contact forces via the maximal dissipation principle (MDP). Prior MDP solvers compute contact forces via convex programming by assuming linearized dynamics integrated using the forward multistep scheme. Instead, we consider the coupled system of nonlinear Newton-Euler dynamics and MDP, which is time-integrated using the backward integration scheme. We show that the coupled system of equations can be solved efficiently using the projected gradient method with guaranteed convergence. We evaluate our method by predicting several locomotion trajectories for a quadruped robot. The results show that our NMDP scheme has several desirable properties including: (1) generalization to novel contact models; (2) superior stability under large timestep sizes; (3) consistent trajectory generation under varying timestep sizes.


I. INTRODUCTION
Articulated body simulation is an indispensable component of robot motion planning, optimal control, and reinforcement learning (RL). Their governing dynamic equations, i.e. the recursive Newton-Euler's equation [9], and discretization schemes have been studied for decades. However, efficient and accurate contact handling is still a challenging problem studied extensively by recent works [28], [35]. To predict robot motions under simultaneous Coulomb frictional contacts, the two most widely-used formulations are the linear-complementary problem (LCP) [30] and the maximal dissipation principle (MDP) [7]. From a computational perspective, LCP incurs an NP-hard problem while MDP identifies contact forces with the solution of a cheap-tocompute convex program. As reported by [8], MDP-based contact handler achieves the best stability and computational efficiency. Moreover, MDP can encode novel contact models as arbitrary convex wrench spaces, which enables learning contact models from data [34], [35].
The stability region of MDP is shown to be up to ∼10 ms according to [32]. Beyond the stability region, the predicted trajectory can either blow-up or drift significantly from the ground truth. Such small stability region not only increases computational cost but also induces problems of vanishing or exploding gradients [18]. In contact-implicit trajectory optimization [23], [29], for example, the problem sizes grow linearly with the number of timesteps and the cost of a Newton-type method grows superlinearly as a result.
We present a non-convex MDP (NMDP) integrator that: (1) is stable under large timestep sizes; (2) generates consistent contact forces under the MDP formulation; and (3)   convex wrench space vx(θ) update, and contact force w update) as a coupled system of nonlinear equations, which is solved using a novel projected gradient method with guaranteed convergence.
generalizes to position-dependent contact models. Prior MDP solvers rely on linearized dynamic systems, so that the kinetic energy becomes a quadratic function of the contact forces which can be solved as a convex QP. However, the truncation error of linearization can grow arbitrarily with larger timestep sizes. Our NMDP solver eliminates the truncation error by formulating the nonlinear recursive Newton-Euler's equation and the wrench space as a function of the robot pose as a coupled system of nonlinear equations timeintegrated using the backward-Euler scheme ( Figure 1). The method can inherently account for novel contact models by using the convex shapes as feasible constraints in MDP, with nonlinear dependence on robot pose. To solve this coupled system we propose using the projected gradient method (PGM). We prove that PGM converges under sufficiently small timesteps and show that it empirically converges under large timesteps. An adaptive inner time-integration scheme guarantees that NMDP solves any (primary) timestep size in finite time. We evaluate our method by predicting walking and jumping trajectories for the JPL Robosimian and Spider quadruped robot. The results show that NMDP has superior stability under larger timesteps as compared with conventional MDP solvers. In addition, the predicted walking speed and jumping height are more consistent under various timesteps.

II. RELATED WORK
We review related work on articulated body dynamics, contact handling, and generalized contact models.
Articulated Body Dynamics: Three classes of time discretization schemes have been independently developed for articulated bodies' equation-of-motion. First, variational integrators (VI) [17], [20] discretize the Lagrangian function and then derive the discrete Euler-Lagrange equation. VI preserves momentum and energy symmetry under large timestep sizes. Second, linear multi-step integrators [6] discretize the equivalent Newton-Euler's equation in the configuration space. These integrators are very efficient to evaluate using the Articulated Body Algorithm (ABA) [9]. Third, highorder collocated, position-based integrators use an equivalent form of the Newton-Euler's equation known as positionbased dynamics (PBD) [4], [26], where the main difference is that the discretization is performed in the Euclidean space. Position-based integrators are stable under large timestep sizes but they do not preserve symmetry.
Contact Handling: Sequential contact models [12], [22] have a significantly limited stability region due to the stiffness of contact forces. Models allowing simultaneous collisions and contacts have larger stability region, especially using implicit time stepper [1], [30]. LCP [2], [30] and MDP [7], [16] are the most popular implicit formulations for simultaneous frictional contacts. Solving complementary conditions due to LCP is NP-hard and can sometimes be infeasible [24]. The MDP relaxes the complementary constraints by allowing any contact forces in the frictional cone to be feasible. However, the stability region of time-stepper is still limited by the linearization of dynamic systems using either LCP or MDP. In [1], [26], dynamics with frictionless contacts are reformulated as an optimization and linearization can be avoided, but these results cannot be extended to frictional cases. In [25], the frictional force is modified and then reformulated as an optimization, but the modified variant cannot handle static-sliding frictional mode switches. In [5], a modified BDF2 scheme is proposed to achieve second-order accuracy in time-integration under frictional contacts, but linearization is still needed. Unlike these methods, we analyze the feasibility of contact handling without linearization for both normal and frictional forces.
Generalized Contact Models: Although the Coulomb frictional model is sufficient for most scenarios involving only rigid objects, other contact models are needed for several reasons. To model the unknown continuous force distribution between a planar object and a flat ground, a general convex wrench space is learned from real-world data in [34]. In other works [15], [19], [33], [35], articulated robots are walking on or swimming in deformable environments with granular or compliant materials. Hu [15] simulated both the granular material and the robot using finegrained finite element method, which is more than 1000× slower than a standalone articulated body simulation. In [19], [33], the Coulomb frictional model is replaced with analytic and empirical force models. Although these models are cheap to compute, they cannot capture the static-sliding frictional mode switches. Zhu [35] used a similar approach as [34] and learned a robot-pose-dependent contact wrench space. Static-sliding frictional mode switches can thereby be modeled using MDP solver with the learned wrench space as constraints. By extending MDP, our NMDP solver can handle any generalized force models [34], [35] in the form of robot-pose-dependent contact wrench spaces.

III. ARTICULATED BODY DYNAMICS
In this section, we briefly review two prior formulations of articulated body dynamics and their corresponding discretization schemes: the recursive Newton-Euler's equation and position-based dynamics. Both schemes can be extended to derive NMDP solvers.

A. Recursive Newton-Euler's Equation
The continuous Newton-Euler's equation under generalized coordinates takes the following form: Here H is the generalized mass matrix, θ is the robot's configuration vector, C(θ,θ) is the Coriolis and Centrifugal force, X(x, θ) is the forward kinematic function bringing a point x from the robot's local coordinates to the global coordinates, C is a set of points in contact with the environment, f x is the external force on x in world coordinates, and finally τ is the joint torque. Remark 1: We assume contacts are realized by external forces f x ∈ R 3 . More general contact models such as [35] require external wrenches f x ∈ R 6 . In this case, we can replace ∇ θ X(x, θ) with the Jacobian matrix in R 6× θ and all the following analysis applies.
To discretize a dynamic system, the linear multistep method uses finite difference approximations for all variables. We illustrate this method with first-order finite difference schemes and higher-order schemes can be applied following a similar reasoning. We introduce two variables θ − and θ −− . We assume that θ − is the robot configuration at current time instance, θ −− is the robot configuration ∆t seconds before (∆t is the timestep size), and θ is the tobe-predicted robot configuration after α∆t seconds. Here α ∈ (0, 1] is an additional parameter for timestep size control and we use subscripts to denote functions that are dependent on α. Since NMDP solver requires sufficiently small timestep sizes to converge, we use α to ensure this condition holds. Under these definitions, we can approximate: Plugging these approximations into the Newton-Euler's equation, the forward-Euler integrator takes the following form: which is a linearized dynamic system in θ, f x , τ . Instead, the backward-Euler integrator evaluates H, C at time-level α∆t instead of current time instance, resulting in a nonlinear system of (1) and (2). This system is not guaranteed to have a solution, unless a small enough timestep size is used.

B. Position-Based Dynamics
PBD reformulates the governing equation-of-motion as: where we define: and the integral in I α is taken over the entire robot R. If we assume that θ is a continuous trajectory θ(t) and θ = θ(t + α∆t), θ − = θ(t), θ −− = θ(t − ∆t), it has been shown in [26] that (4) will converge to (1) as ∆t → 0. This integral can be evaluated analytically in a similar way as deriving the generalized mass matrix. By comparison with (1)+(2), (4) is always solvable under arbitrarily large timestep sizes because it is integrable. In other words, solving for θ is equivalent to the following optimization: argmin Note that we assume f x is a constant in our derivation for the integrability of E α (i.e. PBD dynamics can be written More generally, PBD can still take an integrable form when the external forces are conservative. In scenarios with dissipative force models such as Coulomb frictional forces, both integrability and PBD's feasibility guarantee are lost, just like Newton-Euler's equation. In this work, we propose an algorithm that solves the system of nonlinearity equations with dissipative force models with guaranteed solvability.

IV. NONCONVEX MDP
Our main idea is to combine backward time-integration and frictional contact force computation. In an MDP solver, the force at each contact point f x belongs to a convex feasible space. We assume that the feasible space is a polytope with a set of vertices denoted as v j Here V x is the number of vertices used to model the polytope at contact point x. We assume that all the vertices v j where w x is the weights of convex combination and 1 is an all-one vector.

Remark 2:
The assumption of feasible contact force being a polytope is essential for our convergence proof. Under this assumption, we will extensively use the property that f x has a bilinear form of v x w x , where w x is bounded and v x is sufficiently smooth.

A. NMDP Formulation
When modeling an inelastic rigid contact, v x is set to the vertices of the linearized frictional cone if X(x, θ) is in contact or penetrating the environment, and v x is set to zero otherwise. However, the switch between the in-contact and off-contact state is non-differentiable which is undesirable in applications such as differential dynamic programming [32] and trajectory optimization [23]. Therefore, we assume that v x is a robot-pose-dependent, differentiable function v x (θ). This formulation is compatible with the recently proposed learning-based granular wrench space model [35] and can potentially generalize to other contact models. To determine the weights w x , MDP solves an optimization that minimizes the kinetic energy at time instance α∆t. Conventional MDP solver uses the linearized dynamic system Equation 3 and discretizes v x at θ − , resulting in a QP problem. Instead, our NMDP scheme uses the backward-Euler integrator Equation 1 and discretizes v x at θ. As a result, we need to solve the following nonlinear constrained optimization: argmin where we assume the use of recursive Newton-Euler's equation and w is a concatenation of all w x . In the rest of the paper, we propose two algorithms to solve (6) and analyze their convergence.

B. NMDP Solver
Since (6) is a general nonlinear constrained optimization, it can be solved using general-purpose optimizers such as the interior point method [21]. However, these methods are not guaranteed to converge to a first-order stationary point due to infeasibility. Instead, we consider two variants of the projected gradient method (PGM), which we prove to converge to a first order stationary point. PGM starts from a feasible initial guess and updates a search direction of w by solving: argmin If ∇ θ G α is non-singular, then (7) is equivalent to the following QP: argmin where we use γ to facilitate line search. The matrix 1 is a concatenation of constraints that w x sums to less than one on each contact point x. After solving for a new w ← w + ∆w, we update θ by projecting it to the G α (θ, w) = 0 manifold using the following recursion: Note that we have only used the first-order derivatives of G α in (8) so the PGM has linear convergence speed at best. Our second version of PGM differs in that we ignore all gradients of the function v x , i.e. zeroth-order update for v x . This requirement is inspired by the recent work [35] where the contact wrench space is learned from real-world data. In this case, computing derivatives of v x involves costly backpropagation through a learning model, e.g. neural networks, sublevel sets of high-order polynomials [34], or radial basis functions [35]. Mathematically, the derivatives of v x only occurs in ∇ θ G α and we denote its zeroth-order, inexact variant as: Using ∇ θḠα , we derive the following, inexact counterpart of QP (Equation 8): The pipeline of both first-and zeroth-order PGM is outlined in Algorithm 1.
Return θ k , w k V. CONVERGENCE ANALYSIS We analyze the convergence of Algorithm 1 in both firstand zeroth-order cases. PGM cannot proceed if ∇ θ G α is rank-deficient and does not have an inverse. In addition, the manifold projection substeps in PGM can diverge without using line-search strategies. Finally, the outer-loop of the zeroth-order PGM can fail to converge by using an inexact gradient. We take the following three assumptions to show that first-order PGM and zeroth-order manifold projection are well-defined and convergent: To show that zeroth-order PGM is also convergent, we need an additional assumption: A 5.4: ∇ w G has full row rank. Remark 3: A 5.1 and A 5.2 can be satisfied by choosing appropriate parameterizations of robot joints. When all the hinge joints are parameterized using Euler angles, then A 5.1 is satsified. A 5.2 requires that the kinetic energy K(θ) is strictly convex at θ − . In other words, for any infinitesimal perturbation δθ, there must be some points x ∈ R undergoing infinitesimal movements δx with non-vanishing lim δθ→0 δx δθ. This assumption can only be violated when the robot is suffering from Gimbal lock of Euler angles, which can be easily resolved by moving the singular point away from the current configuration.
Remark 4: A 5.3 indicates that NMDP can only handle smooth contact force models. Stiff and penetration-free contacts between two rigid objects cannot be handled by our method. However, smooth force models are essential for gradient-based motion planning and control [23], [27], [32]. In addition, stiff contacts can be approximated by smooth contacts. For example, a linearized frictional cone is a polytope with vertices being v i x = n + t i µ, where n is the contact normal and t i is a direction on the tangential plane. We modify this definition to satisfy A 5.3 by setting v i x = d(X(x, θ)) 3 (n + t i µ), where d(X(x, θ)) is the penetration depth of the contact point x. This force model can be made arbitrarily stiff by scaling v i x with a big constant. Remark 5: A 5.4 is a major limitation of the zeroth-order PGM. Note that v x = 0 and thus ∇ w G has a zero rank when a contact point x does not penetrate the environment. However, once the penetration depth becomes non-zero, then v x ≠ 0 and ∇ w G can have a full row rank. A 5.4 disallows such jumps in the rank of ∇ w G. A simple workaround is to slightly modify the contact model by allowing small, non-zero contact forces even when robot is not in contact with the environment. One method to satisfy A 5.4 is to set where ζ is a small positive constant. In our implementation, we ignore A 5.4 and have never observed divergence behavior due to the violation of this assumption.
Under the above assumptions, our main results are:  such that for all α ≤ α 5 , the zeroth-order Algorithm 1 will generate a monotonically decreasing sequence of {K(θ k )} where each θ k satisfies G(θ k , w k ) = 0.
The proofs of these results are deferred to Section IX. Both results imply that timestep sizes cannot be arbitrarily large, otherwise PGM can fail to converge. In addition, the divergence behavior of PGM can only happen in the manifold projection substep when the norm G α , Ḡ α does not decrease after apply Equation 9 or Equation 11, which is an indicator of the use of smaller timestep sizes. As a result, we can design a robust articulated body simulator using adaptive timestep control as illustrated in Algorithm 2.

A. NMDP Working with PBD
Our analysis and formulation assumes the use of Newton-Euler's equation. An equivalent form of NMDP can be formulated for the position-based dynamics via a new definition of K(θ) and G α as follows: argmin and all the convergence analysis applies to Equation 6 and Equation 12 alike. We refer readers to Section IX for all the proof.

VI. EVALUATIONS
We evaluate the performance of NMDP in various scenarios. We implement both versions of NMDP (Equation 6 and Equation 12) using C++ and Eigen [13], where the optimizations can be solved using both first-and zerothorder PGM. All the matrix inversions in manifold projection are solved by a rank-revealing LU factorization. As long as the factorization detects that the matrix is near singular (i.e. A 5.2 is violated) or the norm G α , Ḡ α does not decrease, we restart PGM with smaller timestep sizes. In each outer loop of PGM, a QP is solved and the problem data of these QP are quite similar. We use the parametric QP solver [10] that can make use of these similarities to accelerate computation. Finally, we set = 10 −6 , η = 1.5, ζ = 10 −3 .
As illustrated in Figure 2, we conduct experiments on the Robosimian by having different simulators to track a prescribed robot walking or jumping trajectory using the stable PD controller [31]. The stable PD controller is consistent with the backward-Euler integrator, which uses θ,θ instead of θ − ,θ − as the target state to be tracked. We compare the performance of the following simulators: • NE-NMDP-PGM/NE-NMDP-ZOPGM: Equation 6 solved using first-/zeroth-order PGM.
Stability Under Large Timestep Sizes: We track a robot jumping trajectory that uses symmetric poses. Since the Robosimian's body shape is also symmetric, the torso in the tracked trajectory should have zero tilt angles from the vertical axis, which is our groundtruth. In Figure 3 (ab), we plot the torso's tile angle predicted by NE-MDP and NE-NMDP-PGM under different timestep sizes. NE-MDP is only stable when ∆t ≤ 7ms and the simulator explodes under larger ∆t. Even when ∆t ≤ 7ms, the tile angle suffers from severe oscillation. In comparison, NE-NMDP-PGM is stable when ∆t increases from 5ms to 50ms and the predicted tile angle oscillation is relatively small. We further plot the tile angle sequence predicted by the other three variants of PGM in Figure 3 (cde), the oscillations are consistently small. When using PGM with ∆t ≤ 50ms, we have never experienced any divergence behaviors so Algorithm 2 is never needed. But divergence happens in ZOPGM and we observe larger tile angle oscillation in Figure 3 (ce).
Consistent Prediction: The tracked robot jumping trajectory lasts for 10 seconds with 5 repeated jumping behaviors. As a result, the expected torso height should be a periodic function. In Figure 4, we plot the torso height trajectory predicted using the five methods. With ∆t ≤ 7ms, the trajectories generated by NE-MDP are suffering from relatively large variations. While all four PGM variants can consistently predict a periodic function when ∆t increases from 5ms to 50ms. A similar result is observed in the walking trajectory. As shown in Figure 5, the groundtruth walking distance is a linear function of time. The trajectories predicted using NE-MDP exhibit a large variations with different timestep sizes. The consistency of NE-NMDP-(ZO)PGM are much better and that of PBD-NMDP-(ZO)PGM are the best. Compared with NE-NMDP-(ZO)PGM, the additional stability and consistency of PBD-NMDP-(ZO)PGM are presumably due to the fact that Euclidean space discretization is more accurate than that in the configuration space [26].
Computational Cost: We summarize the computational performance by collecting and analyzing all the timesteps in the four trajectories of Figure 2 simulated at ∆t = 50ms. Figure 6 (a) profiles the instantaneous framerate, of which MDP is the most efficient involving a single QP solve. PGM does not incur a significant sacrifice in framerate while ZOPGM is significantly slower. Figure 6 (b) profiles the number of outer iterations of Algorithm 1 until convergence and Figure 6 (c) plots the average number of outer iterations against ∆t. These figures show that ZOPGM is an approximately one order of magnitude slower than PGM, but ZOPGM provides the extra convenience that analytic derivatives of v x (θ) is not needed. Finally, Figure 6 (d) shows the smallest timestep size chosen by Algorithm 2 for PGM and ZOPGM. At ∆t = 50ms, PGM is always convergent. ZOPGM is convergent for most timesteps, but ∆t needs to get down to 25ms during some critical time instances (e.g. when robot changes contact state).

VII. CONCLUSION & DISCUSSION
We present NMDP, a backward-Euler time integration scheme for articulated bodies under generalized contact models. The key to our formulation is the representation of contact forces as a convex combination of vertices of the feasible contact wrench space. To model generalized contact models, we assume that these vertices are dependent on the robot pose. Following the idea of backward-Euler time integrator, we discretize both the articulated body dynamics and the vertices of contact wrench spaces at the next time instance instead of the current one. We solve the constrained optimization using a projected gradient method. Our analysis proves that NMDP has guaranteed convergence under small timestep sizes and a robust simulator can be built using an adaptive timestep size control algorithm. Empirically, we show that this scheme has better stability under large timestep sizes and consistency over varying timestep sizes.
We plan to address several limitations in our future work. First, the main idea of NMDP is not limited to articulated bodies and can be extended to high-dimensional dynammic systems such as deformable objects. However, it requires exact inversion of the constraint Jacobian, which is impractical in high-dimensional scenarios. Second, we assume a small set of known contact points. When new contacts are detected, we do not handle them in our implementation. Third, the generalized contact model needs to be smooth and cannot provide collision-free guarantees. Finally, we plan to evaluate the performance of optimization-based motion planners and controllers using NMDP as the underlying integration model.

VIII. ACKNOWLEDGEMENT
This work is partially funded by NSF Grant #1911087 and authors thank Mengchao Zhang for proofreading the paper.

IX. APPENDIX: PROOF
We define some convenient shorthand notations.
Note that all variables or functions that depend on α will be labeled using a subscript, and vice versa.

A. Continuous Manifold Projection
We begin our analysis by showing that θ satisfying G α (θ, w) = 0 can be arbitrarily close to θ − by using sufficiently small α. This can be proved by analyzing the following Lyapunov candidate: We prove basic properties of boundedness and convexity: Lemma 9.1: Assuming A 5.1,A 5.2,A 5.3, there exists an r > 0, such that for all feasible w, the following properties hold within B(θ − , r): Proof: These are immediate results of continuity of functions and singular values of matrices. 1): 2): 3),4): Also in the region B(θ − , r), we have that Equation 9, Equation 7, and Algorithm 1 are well-defined because Lemma 9.1 implies that ∇ θ G α is invertible: Corollary 9.2: Assuming A 5.1,A 5.2,A 5.3, for r chosen as in Lemma 9.1, Algorithm 1 is well-defined as long as We can easily show that in a small vicinity of θ − , manifold projection always has a solution: Theorem 9.3 (Continuous Convergence): Assuming A 5.1,A 5.2,A 5.3, for r chosen as in Lemma 9.1, there exists α 1 > 0, such that for any α ≤ α 1 and feasible w, the solution to G α (θ, w) = 0 computed using the negative gradient flowθ = −∇ θ V α from initial guess θ − is within B(θ − , r).
Proof: Solution Boundedness: Consider V α (θ, w) − V α (θ − , w), with θ ∈ ∂B(θ − , r). We have strong convexity along line-segment connecting θ, θ + and by Lemma 9.1: Vα(θ, w) − Vα(θ−, w) If we choose small enough α 1 such that the last equation is larger than zero for all α ≤ α 1 , then V α becomes a Lyapunov function of the gradient flow, restricting the solution to B(θ − , r) due to Nagumo's Theorem [3]. Convergence: We still need to show the gradient flow converges to a solution of G α (θ, w) = 0, which is trivial due to the following inequality: We can again choose small enough α 1 such that the coefficient of V α in the last inequality is smaller than zero. Lemma 9.3 implies that, with sufficiently small α, PGM will always generate a sequence that is within B(θ − , r) if manifold projection is solved by exactly time-integrating the negative gradient flow.

B. Discrete First-Order Manifold Projection
In this section, we go beyond Lemma 9.3 and analyze the practical discrete manifold projection algorithm, i.e. Equation 9. Let's define the shorthand notation: We first analyze the property of θ + , i.e. one iteration of manifold projection. We show that the relative change θ + − θ is bounded: Lemma 9.4: Assuming A 5.1,A 5.2,A 5.3, V α (θ, w) ≤ M V where M V is some α-independent constant. For r chosen as in Lemma 9.1, there exists α 2 > 0, such that for any α ≤ α 2 and feasible w, θ + −θ ≤ r 2 if θ + , θ ∈ B(θ − , r).