Robust Pivoting Manipulation Using Bilevel
Contact-Implicit Optimization
Yuki Shirai† , Devesh K. Jha‡ , Arvind U. Raghunathan‡ and Diego Romeres‡

Abstract—In this draft, we present the formulation of bilevel
optimization for performing robust pivoting manipulation. We
derive analytical expressions for stability margin provided byfriction during pivoting manipulation. This margin is thenused in the bilevel trajectory optimization to design an openloop controller that maximizes this stability margin to provide
robustness against uncertainty in physical properties of theobject. The proposed method hass been tested on several pivoting
manipulation scenarios.

I. I NTRODUCTION
Contacts are central to most manipulation tasks as they
provide additional dexterity to robots to interact with theirenvironment. Designing robust controllers for frictional interaction with objects with uncertain physical properties ischallenging as the mechanical stability of the object depends
on these physical properties. Inspired by this problem, weconsider the task of pivoting manipulation in this paper. In
particular, we consider the problem of re-orienting parts withuncertain mass and Center of Mass (CoM) location usingpivoting. We are interested in ensuring mechanical stability
via friction to compensate for uncertainty in the physicalproperties of the objects.
We study pivoting manipulation where the object being
manipulated has to maintain slipping contact with two externalsurfaces. A robot can use this manipulation to reorient parts
on a planar surface to allow grasping or assist in assembly
by manipulating objects to a desired pose (see Fig. 1). Notethat this manipulation is challenging as it requires controlled
slipping (as opposed to sticking contact [2, 1]), and thus it isimperative to consider robustness of the control trajectories.
To ensure mechanical stability of the two-point pivoting in
the presence of uncertainty, we derive a sufficient condition
for stability which allows us to compute a margin of stability.
This margin is then used in a bilevel optimization routine todesign an optimal control trajectory while maximizing thismargin.
II. F RICTIONAL S TABILITY M ARGIN
We briefly provide some physical intuition about frictional
stability (see Fig. 2). First, suppose that uncertainty exists inmass of a body. In the case when the actual mass is lower thanestimated, the friction force at point A would increase while
the friction force at point B would decrease, compared to thenominal case. In contrast, suppose if the actual mass of thebody is heavier than that of what we estimate, then the body
can tumble along point B in the clockwise direction. In thiscase, we can imagine that the friction force at point A wouldFig. 1: We consider the problem of reorienting parts for assembly using
pivoting manipulation primitive. Such reorientation could possibly be required
when the parts being assembled are too big to grasp in the initial pose (suchas the gears) or the parts to be inserted during assembly are not in thedesired pose (such as the pegs). The figure shows some instances during
the implementation of our controller to reorient a gear and a peg.
Robust Bilevel Trajectory Op�miza�on for Pivo�ng
s. t. Kinema�cs, Dynamics
Ini�al and Terminal States
Bounds on Variables
Largest Fric�onal Stability at

marginmarginUncertain Mass

UncertainCoM Loca�onFig. 2: Conceptual schematic of our proposed frictional stability and robust
trajectory optimization for pivoting. Due to slipping contact, friction forcesat points A, B lie on the edge of friction cone. Given the nominal trajectoryof state and control inputs, friction forces can account for uncertain physicalparameters to satisfy static equilibrium. We define the range of uncertainty
in gravitational forces and moments that can be compensated by contacts asfrictional stability.

decrease while the friction force at point B would increase.
However, as long as the friction forces are non-zero, the object
can stay in contact with the external environment. Similararguments could be made for uncertainty in CoM location. The
key point to note that the friction forces can re-distribute at thetwo contact locations and thus provide a margin of stability tocompensate for uncertain gravitational forces and moments.
We call this margin as frictional stability. See [3] for moredetails.
III. ROBUST B ILEVEL C ONTACT-I MPLICIT O PTIMIZATION
We briefly present our open-loop controller formulation
where we incorporate frictional stability in optimization to
obtain robustness under uncertaity of mass and CoM location.
An important point to note is that the optimization problem

TABLE I: Obtained worst stability margins over the time horizons fromoptimization for the peg. Note that the stability margin for the solution of
the benchmark optimization is analytically calculated.
Benchmark optimization
Ours (1) with mass uncertainty
Ours (1) with CoM uncertainty

ϵ∗+ , ϵ∗− [N]0.035, 0.018
0.050, 0.021
N/A∗ , r ∗ [mm]r+−31, 0
N/A38, 0would be ill-posed if we naively consider uncertainty of massand CoM location in static equilibrium since there is no u
to satisfy all uncertainty realization in equality constraints.
Thus, our strategy is that we plan to find an optimal nominal
trajectory that can ensure external contacts under mass or CoM
location uncertainty. In other words, we aim at maximizing
the worst-case stability margin over the trajectory given themaximal frictional stability at each time-step k. We formulatea bilevel optimization problem which consists of two lowerlevel optimization problems as follows:
max∗
x,u,f,ϵ∗
+ ,ϵ−

s. t.

(min ϵ∗k,+ − max −ϵ∗k,− )
kk

(1a)

Fig. 3: (a), (b): Trajectory of frictional stability margin of the peg basedon uncertain mass obtained from our proposed bilevel optimization, baseline
optimization, respectively. (c): Snapshots of pivoting motion for the peg,
obtained from our proposed bilevel optimization considering uncertain mass.
(d): Snapshots of hardware experiments for the peg.
TABLE II: Number of successful pivoting attempts of gear 1 over 10 trials forthe two different methods. To evaluate robustness for objects with unknown
mass, we solve the optimization with mass different from the known object
and implement the obtained trajectory on the object with known mass. Notethat the actual mass of the gear is 140 g.

kinematics, hybrid dynamics, variable bounds, (1b)ϵ∗k,+ ∈ arg max{ϵk,+ : Ak ϵk,+ ≤ bk , ϵk,+ ≥ 0}, (1c)ϵk,+ϵ∗k,− ∈ arg max{ϵk,− : −Ak ϵk,− ≤ bk , ϵk,− ≥ 0} (1d)m = 100 g
m = 110 g
m = 140 g
m = 170 g

Bilevel Optimization
10 / 1010 / 1010 / 1010 / 10

Benchmark Optimization
0 / 100 / 100 / 100 / 10ϵk,−where ϵ∗k,+ , ϵ∗k,− are the largest uncertainty of mass in thepositive and negative direction, respectively, at k given x, u, f ,which results in non-zero contact forces. x, u, f are states (i.e.,box orientation and position of point P in Fig. 2), control
input (i.e., forces at point P ), and external contact forcesfrom point A and B, respectively. Ak , bk represent inequality
constraints associated with frictional stability margin. (1a)is the upper-level objective function, which maximizes thesmallest stability margin over time-horizons by subtracting thestability margin along the positive direction from that along
the negative direction.
The advantage of (1) is that since the lower-level optimization problem are formulated as two linear programming
problems, we can efficiently solve the entire bilevel optimization problem using the KKT condition. Using the KKT
condition and epigraph tricks, (1) becomes a single-level largescale nonlinear programming problem with complementarity
constraints. See [3] for more details.
IV. E XPERIMENTAL R ESULTS
We first show some numerical results discussing how muchrobustness our proposed controller provide over the baseline
controller as shown in Table I and Fig. 3. Fig. 3 shows thefrictional stability for the peg obtained during the trajectories
from the proposed method and the baseline method.
Table I summarizes the computed stability margin fromFig. 3. Our bilevel optimizer finds trajectories that have larger
stability margins for both uncertain mass and CoM location.
The trajectory of stability margin obtained from bilevel optimization considering mass uncertainty is illustrated in Fig. 3.

We also implement our controller using a real 6 DoF manipulator to demonstrate the efficacy of our proposed method forvarious gears and pegs. To evaluate robustness for objects withunknown mass, we solve the optimization with mass different
from the true mass of the object and implement the obtained
trajectory on the object.
Table II shows the success rate of pivoting for the hardware
experiments. We observe that our proposed bilevel optimization is able to achieve 100 % success rates for all 4 mass values
while benchmark optimization cannot realize stable pivoting.
R EFERENCES[1] Francois R Hogan, Jose Ballester, Siyuan Dong, andAlberto Rodriguez. Tactile dexterity: Manipulation primitives with tactile feedback. In 2020 IEEE international
conference on robotics and automation (ICRA), pages
8863–8869. IEEE, 2020.
[2] Yifan Hou, Zhenzhong Jia, and Matthew T Mason. Fastplanning for 3d any-pose-reorienting using pivoting. In
2018 IEEE International Conference on Robotics andAutomation (ICRA), pages 1631–1638. IEEE, 2018.
[3] Yuki Shirai, Devesh Jha, Arvind Raghunathan, and Diego
Romeres. Robust pivoting: Exploiting frictional stability
using bilevel optimization. In 2022 IEEE international
conference on robotics and automation (ICRA). IEEE,2022.