{'fraction_non_alphanumeric': 0.0311737164352315, 'fraction_numerical': 0.009724278682620354, 'mean_word_length': 4.92601375385345, 'pattern_counts': {'":': 0, '<': 2, '<?xml version=': 0, '>': 0, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 3, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
Contact-Implicit Planning and Control for Non-Prehensile Manipulation Using State-Triggered Constraints

We present a contact-implicit planning approach that can generate contact-interaction trajectories for non-prehensile manipulation problems without tuning or a tailored initial guess and with high success rates. This is achieved by leveraging the concept of state-triggered constraints (STCs) to capture the hybrid dynamics induced by discrete contact modes without explicitly reasoning about the combinatorics. STCs enable triggering arbitrary constraints by a strict inequality condition in a continuous way. We first use STCs to develop an automatic contact constraint activation method to minimize the effective constraint space based on the utility of contact candidates for a given task. Then, we introduce a re-formulation of the Coulomb friction model based on STCs that is more efficient for the discovery of tangential forces than the well-studied complementarity constraints-based approach. Last, we include the proposed friction model in the planning and control of quasi-static planar pushing. The performance of the STC-based contact activation and friction methods is evaluated by extensive simulation experiments in a dynamic pushing scenario. The results demonstrate that our methods outperform the baselines based on complementarity constraints with a significant decrease in the planning time and a higher success rate. We then compare the proposed quasi-static pushing controller against a mixed-integer programming-based approach in simulation and find that our method is computationally more efficient and provides a better tracking accuracy, with the added benefit of not requiring an initial control trajectory. Finally, we present hardware experiments demonstrating the usability of our framework in executing complex trajectories in real-time even with a low-accuracy tracking system.


Introduction
Non-prehensile manipulation will be a key capability for robots in both home and industrial environments [40]. However, there is still a lack of reliable methods that can compose contact-rich motions given only a high-level goal. The main challenge is that the planning and control of contact interactions require discrete decisions concerning the time and location of contact mode transitions that impose switching constraints altering the evolution of the system dynamics. To avoid predefined contact schedules, a hybrid problem using mixed-integer programming (MIP) may be solved, however, explicit modeling can be prohibitive as the complexity grows exponentially with discrete variables [33]. Fortunately, many discrete elements can be expressed in terms of continuous variables -such as the distance, velocity, and force in the contact frame -by using complementarity constraints (CCs) which provide an efficient way of including bi-directional conditions in programs without the combinatorial complexity of explicitly doing so. Thus, contact dynamics is defined as a smooth optimization problem with CCs in many physics engines and planning algorithms [38,49,33]. Yet, describing the rich discrete aspects of contact-related problems using only bi-directional statements is restrictive and may be inefficient. We hypothesize that a continuous model that implies uni-directional if conditions would be useful as it would provide a more modular building block. Hence, we propose the use of statetriggered constraints (STCs), first put forward by Szmuk et al. [43], and analyze their performance for contact-implicit planning (CIP) through non-prehensile manipulation examples.

Contributions
The main contributions of our work are: -An automatic contact constraint activation (CA) method that triggers only the contact constraints useful for given task, thus reducing the problem's sensitivity to the number of contact pairs and enabling automatic contact candidate assignment, i.e., all perceived surfaces can be assigned as contact candidates as they are handled efficiently in the optimization. Extensive simulations show that the proposed CA method improves the computational efficiency and the success rate. -We reformulate the Coulomb friction model using STCs and compare it to a baseline based on CCs for discovering tangential forces to rotate a box to random goal orientations. The proposed model is significantly faster and more robust than the baseline. -The STC-based friction model is used to model the contact modes of planar quasi-static pushing and compared to a MIP-based controller. Our method outperforms the baseline both in speed and accuracy. Moreover, the method can plan contact-interaction trajectories given only a desired state trajectory without any heuristics. Although our method may run slower than the learning-based variant [15], we demonstrate experimentally that our STCbased control can plan and track complex trajectories at a frequency high enough for real-time applications.
To the best of our knowledge, this is the first usage of STCs in contact modeling. We show that STCs hold immense promise for modeling discrete elements in this domain and can mitigate the need for explicit combinatorics and heuristics. Our proposed CA and friction methods serve as example applications for STCs. The extensive simulation and hardware experiments prove the efficacy of the proposed methodology.

Related Work
Mixed-Integer Programming The planning and control of contact-rich motions can be achieved by building a hybrid problem with explicit discrete variables and then solved by a MIP approach. In [7,1], MIP-based planners are developed and applied to locomotion on uneven terrains. In manipulation, MIP [14,2] and exhausted tree search [8] have been used to achieve desired object behaviors. Nevertheless, these approaches typically require simplifications and/or heuristics to make them computationally tractable. [15,22,9] introduced various initialization strategies to reduce the computational burden, mostly with data-driven techniques.
Contact-Implicit Trajectory Optimization Alternatively, contact-interaction trajectories can be planned by incorporating a differentiable contact model into a trajectory optimization framework, i.e., contact-implicit trajectory optimization (CITO). [26,25] have shown complex contact-rich behaviors can be synthesized for animated characters using convex optimization with soft constraints by sacrificing physical realism. In robotics applications, where motions must be physically accurate, CCs are widely used to model inelastic rigid-body contacts with Coulomb friction. In [49] the non-smooth trajectory optimization problem with impacts and discontinuities are transcribed into a bi-level program with CCs. Posa et al. [33] solved this problem simultaneously using direct collocation. Similar methods have been proposed for planar manipulation [10] and dynamic pushing [37]. In [24,21], hierarchical strategies using warm-starting to improve the computational efficiency are presented, while [19,32] propose methods to improve the integration accuracy of such numerical schemes. Other works have focused on replacing CCs for more efficient direct programs, e.g., [48,6,39]. The optimization problem can be solved faster than direct optimization via Hessian-approximating differential dynamic programming (DDP) variants, such as iterative linear quadratic regulator (iLQR) [17], typically with smooth contact models as these approaches cannot easily handle constraints. In [44,27], smoother fragments of CCs are used with DDP variants, and the potential to run CITO as model predictive control (MPC) is demonstrated. [28] utilized an explicit smooth contact model to enable real-time MPC for highly-dynamic quadruped motions.
However, these methods usually require either a good initial guess and/or tedious tuning due to the unreliable convergence of DDP methods. Recently, we proposed a CITO framework based on a variable smooth contact model [30] that decouples the relaxation from the contact model by injecting smooth virtual forces into the underactuated dynamics with frictional contacts. Hence, both nonlinear programming and DDP variants can be used to solve the problem, as shown by [20,31,46,29]. While the methods developed here are applicable to our previous work, we build upon a CC-based CITO framework similar to [33] as it is more widely used in the related literature and thus a more relevant comparison.
Planar Pushing The mechanics of planar pushing in the presence of friction was first studied in [23] which focuses on the rotational direction and the object's center. In [18], the mechanics for stable pushing is studied. More recently, [50] have shown that with a sticking contact, Dubins path can plan trajectories efficiently. In [14] a hybrid MPC framework is proposed to track a pushing trajectory without limiting the contact mode to sticking only, instead the contact mode for each time step is defined as a discrete variable. To avoid the combinatorial problem's computational, convex problems are solved for a fraction of potential contact schedules for the prediction horizon, and the solution is determined comparatively. In [15], an aggregated contact mode selection method is proposed to reduce the complexity of MIP and to eliminate the need for a predefined contact mode schedule. They proposed a learning method to predict the proper contact mode to achieve a higher control frequency. However, this approach is not very versatile as it requires re-training for object shape variations.
State-Triggered Constraints STCs were first introduced in [43] as a more modular, uni-directional alternative to CCs and were used to avoid MIP and enable real-time performance for a rocket landing task with state-dependent constraints. In [42], this concept is extended into a more general form called compound STCs showing that more articulate trigger and constraint conditions can be obtained by applying Boolean logic operations. STCs have been successfully applied to real-time rocket landing [34,35] and quadrotor path planning [41].

Contact Model
Following [38], a contact impulse due to a frictionless inelastic collision between two rigid bodies can be modeled by 0 ≤ φ(q) ⊥ λ n ≥ 0; where the non-negativity condition for the signed distance φ(q) prevents interpenetration between bodies, and the non-negativity condition for the normal contact force λ n ensures that the contact bodies can only push each other. The frictional forces λ t ∈ R 2 can be modeled using the relative tangential (slip) velocity in the contact frame ψ(q,q), the friction coefficient µ and by splitting tangential force into directional components λ + t , λ − t ≥ 0: where γ is an auxiliary variable corresponding to the absolute value of the slip velocity. It is noteworthy that this formulation is based on the maximization of the dissipation of kinetic energy subject to pyramidical friction cone constraints, see [19] for a derivation. However, the stationarity condition is neglected, and this leads to not being able to counteract external forces to prevent slippage when the slip velocity is zero. Nevertheless, these constraints are sufficient to ensure that the tangential forces selected by the planner are bounded by the friction polyhedron when sticking and counteracting at the boundary when slipping.

Contact-Implicit Trajectory Optimization
The CIP problem can be transcribed into a nonlinear program (NLP) for N time steps of size h using a backwards Euler discretization of the underactuated dynamics for an n v n a +n u degrees-of-freedom (DOF) system with n a actuated and n u unactuated DOF: subject to: where q ∈ R nq andq ∈ R nv are the generalized positions and velocities of the system; M(q) ∈ R nv×nv and c(q,q) ∈ R nv denote the mass matrix and the Coriolis, centrifugal, and gravitational terms; τ ∈ R na is the vector of generalized joint forces; S a ∈ R nv×na is the selection matrix for the actuated DOF; and λ c ∈ R 3nc is the vector of contact forces for n c contacts that are projected onto the joint space through the contact Jacobian J c (q) : ] are the state, control, force, and slack trajectories; and τ L , τ U , x L , and x U are the lower and upper control and state bounds. The slack variables s are introduced to relax the CCs (including those for frictional forces) and penalized in the cost by an exact penalty function, as described in [19,3]. The constraints are evaluated element-wise [33].

Quasi-Static Planar Pushing
In this work, we consider a single point pusher-slider system under the assumptions of quasi-static motion, Coulomb friction, and uniform pressure distribution over the support plane by following the formulation in [14]. We model the dynamics of planar pushing using an ellipsoidal approximation of the limit surface [12]. We consider three contact modes between the pusher and the slider: sticking, sliding left, and sliding right. Each mode imposes a different set of friction constraints on the system. The planning problem can then be formulated as an MIP by linearizing the dynamics given desired state and control trajectories and a contact mode schedule. This framework is used to control the system in a receding horizon fashion such that the deviations from the nominal trajectories are minimized. We refer the reader to [15,14] for details.

State-Triggered Constraints
An STC consists of a strict inequality trigger condition g(x, u) < 0 and an inequality constraint condition c(x, u) ≤ 0 such that That is, the constraint c(x, u) ≤ 0 becomes active if the trigger condition g(x, u) < 0 is met. To include this logical implication in a smooth optimization problem, it must be represented using continuous variables, by introducing an auxiliary variable σ ∈ R ++ and the following constraints σc(x, u) ≤ 0.

Automatic Contact Constraint Activation
In the CC-based CITO, the constraint space grows with the number of contact pairs. Yet, at any instance, only a fraction of contact pairs are useful. Thus, we propose an automatic CA mechanism based on STCs that eliminates unnecessary contact pairs. This concept, based on a simple heuristic for planar pushing, is intended to showcase the versatility of STCs in this domain. Fig. 1 illustrates the CA mechanism for a planar pushing scenario where d is the vector from the box's current location to the desired. In this case, contact candidates are typically defined by pairing each end effector with each surface of the manipuland. However, the contact pairs that cannot generate normal forces to aid task completion can be deactivated by bounding the corresponding normal forces to zero. This is achieved by replacing the CCs in (2e) by the following STCs where N [n 1 , ..., n nc ] ∈ R 3×nc and n denotes the surface normal. Note that when the normal force is set to zero, the tangential forces are also suppressed.
In addition to selectively activating contact pairs for manipulation, this mechanism can be used to activate contact constraints efficiently in a cluttered environment. For example, the distance between the manipuland and the environment being less than a threshold, d thresh , can be used as the trigger condition to activate important contact pairs on the fly, i.e., This is somewhat similar to the collision constraint relaxation used in [36] when the distance is above a safety margin. It should be noted that the activation of constraints based on task utility, i.e., the addition of (5) into (2), is performed only for the contact pairs between the robot and the manipuland. Whereas, the activation of contact pairs that have a distance less than the threshold, i.e., the addition of (6) into (2), is performed for all contact pairs. Thus, contact interactions are enabled between all contact pairs throughout the optimization but in a computationally-efficient way. ‡ ‡ Experiments demonstrating the distance-triggered contact pairs can be found in the accompanying video.

A Reformulation of Coulomb Friction
The Coulomb friction model can be expressed in terms of if-then conditions as ψ(q,q) = 0 ⇒ λ t = −sign(ψ(q,q))µλ n .
Such logical implications can be conveniently expressed using the following STCs along with a pyramidical approximation of the boundary constraint In contrast to the CC-based formulation in (1), taking the absolute value of the slip velocity γ is not required here, since non-negativity constraints do not need to be enforced when using STCs. Moreover, the Coulomb friction law described in (7) is expressed sufficiently using only uni-directional implications. In the remainder, (1) is used for the CC-based friction model, and (8) is used for the STC-based friction model. Note that we use these friction models to model the friction only in contacts between the robot and the manipuland and not for support contacts due to the lack of physical fidelity described in 2.1.

Quasi-Static Pushing Planning and Execution
Both of the presented friction models can be employed to capture the switching nature of the quasi-static pushing without discrete modeling such that the MIPbased pushing controller [14] can be re-formulated as a direct NLP given only a desired state trajectory X d X +X: where Q N , Q, and R are weight matrices that adjust the penalization of the state deviation and the control effort. Either friction model can be incorporated by replacing g(u i ) ≤ 0 by the corresponding constraints, (1) or (8). To exploit the structure of the problem, we use a large-scale sparse solver that allows warmstarting, i.e., SNOPT [11]. At each time step during execution, the initial control input u 0 is executed, and the shifted solution is used as the initial guess for replanning in a receding-horizon fashion. This formulation also differs from the MIP approach as the optimization variables are the absolute state and control values instead of their variations, as  shown in Fig. 2. A significant advantage of this approach is that a predefined contact mode schedule and control trajectory are unnecessary meaning the NLPbased approach can be used to plan a contact-interaction trajectory given only a desired state trajectory and using a trivial initial guess where the robot and the object are static.

Experiments
For the software implementation, the dynamics and the analytical gradients [4] are calculated using Pinocchio [5]. The NLP solvers IPOPT [45] and SNOPT are interfaced using IFOPT [47]. The MIP is solved using Gurobi [13]. The computations were run on an AMD Ryzen 7 2700x processor.

Dynamic Pushing
Two non-prehensile manipulation tasks are designed to evaluate the proposed methodology: a translation-only task for the CA method and a rotation-only task for the friction models. We use a standard 6-DOF robot arm, UR3e from Universal Robots, and a cube with a side length of 0.1 m and a mass of 0.6 kg. The friction coefficient is 0.6 for contact pairs between the box and robot and 0.2 between the box and ground. The weights in (2) Table 1 shows the computation time statistics for converged runs and the success rates with and without CA for both solvers. When using IPOPT, significant advantages regarding computational time, on average a reduction of over 70%, and more than twice the success rate are achieved with CA. With SNOPT, the success rate is lower, however the overall performance with CA is still better. This is significant as it can enable automating contact candidate assignment from perception data. That is, all detected surfaces can be assigned as contact candidates since the number of contact pairs would not be an issue when the constraints are handled efficiently on the fly. Friction Model We compare the STC-and CC-based friction models for the discovery of tangential forces considering a rotation-only task where a box that is attached to the ground via a revolute joint is commanded an orientation. A contact pair is defined between the robot's end effector and the center point of the box's surface that faces the robot. Hence, the robot can exert forces only on the center point of the vertical surface. Since this point aligns with the box's center of mass, frictional forces must be employed to complete the task. The box is initialized at 0 rad, and 100 goals are uniformly sampled from [−1, 1] rad. The right half of Table 1 shows the computation time statistics for converged runs and the success rates for both models and solvers. On average, the STCbased model outperforms the baseline both in speed and robustness. We believe that this is owing to the reduction of the number of binding constraints. In other words, the Coulomb friction law -which, in fact, consists of if conditions -can be more naturally represented using STCs. Whereas, the CC-based model is trying to capture the same by using excessive bi-directional conditions.
(a) Computational time comparison.
(b) Tracking error comparison. Hardware Experiments The proposed framework is also validated using the hardware setup shown in Fig. 6. A Kinova Gen3 robot arm is used with a 3D printed pusher to manipulate a square metal block of length 0.1 m and mass 1 kg. The friction coefficient between the block and the ground is estimated as 0.4. As the robot arm is position controlled, the control inputs solved from  the optimization are mapped to pushing velocity through a linear mapping [15]. A camera mounted on the arm and AprilTag 3 [16] are used to estimate the slider's pose. Without occlusion, the localization error is within 0.3 mm. The controller parameters used in simulation are maintained except that R is reduced to 0.1 diag(1, 1, 0.01) to accommodate planning from scratch. We first test this framework for tracking a straight line and a circle under external forces. The result for tracking the straight line is shown in Fig. 4(b). It is seen that the controller reacts quickly and drives the slider back to the reference trajectory by adapting friction modes as necessary upon perturbation. Fig. 5(a) shows the result of tracking a circle of radius 0.15 m for 5 consecutive laps at a uniform desired reference angular velocity of 0.3 rad/s. In this case, an average tracking error of 0.019 m is obtained with an average computational time of 0.0255 s. While the controller is able to achieve stable circle tracking for 5 laps, the actual path constantly deviates from the reference path on the left and right end of the circle. We attribute this to the variations of task-space control performance and object tracking accuracy across the workspace. Referring to the hardware setup in Fig. 6, the right end and left ends of the reference circle are the positions where the robot gets close to its operation range limits. The results for circle tracking with external perturbations are shown in Fig. 5(b) proving that the controller can reject disturbances even when state deviations are large.
To further evaluate the proposed controller's generalization, we test it for randomly-selected complex trajectories. The trajectories of shapes "n", "e", and "u" are used as references. Snapshots from the hardware experiments are depicted in the top row of Fig. 6, and the bottom row shows the resulting trajectories. * Doing this using the baseline would be challenging due to the high computational cost of MIP [14] and the need for re-training for the learning-based model [15]. We obtain average tracking errors of 0.0313 m, 0.0332 m, 0.0310 m for the "n", "e", and "u" shapes with an average computational time of 0.017 s. * Please see the accompanying video for the full motion. It is noteworthy that despite the low-accuracy tracking, we have not observed any performance issues qualitatively; nevertheless, the quantitative tracking errors should be affected by this. We believe that a more accurate motion-capture system would improve the controller's performance.

Conclusion
We have presented two key innovations for contact-implicit planning using STCs. First, we have introduced an automatic contact constraint activation method that enables the system to disregard unpromising contact pairs on the fly. We believe this will enable systems to explore a much larger solution space while maintaining efficiency and thus could prove important in applications such as manipulation in clutter. Second, we have proposed a re-formulation of Coulomb friction. Both approaches have been evaluated for dynamic pushing scenarios in simulation against CC-based baselines and found to provide significant improvements in computation time and success rate. Finally, by leveraging the proposed friction model, we formulate the quasi-static planar pushing problem as a direct NLP that can be solved fast enough to be run as an MPC. The proposed controller has been compared to a MIQP-based approach and shown to achieve higher control frequency and better tracking accuracy in simulation. Moreover, the proposed method has been demonstrated on hardware for reactive planning of complex trajectories in real-time without any heuristics.