Python-based Open Source Package for
Optimization of Contact-rich Systems
Arvind U. Raghunathan1 , Devesh K. Jha1 and Diego Romeres1

Abstract—P Y ROBO COP is a Python-based package for control, optimization and estimation of robotic systems describedby nonlinear Differential Algebraic Equations. In particular, thepackage can handle systems with contacts that are described bycomplementarity constraints and provides a general frameworkfor specifying obstacle avoidance constraints. P Y ROBO COP provides automatic reformulation of the complementarity constraints
to perform optimization of robotic systems. The package isinterfaced with ADOL-C [1] for automatic differentiation withsparse derivatives and IPOPT [5] as solver.

User

PyRoBoCOP

Environment
Dynamics
Constraints
Cost Function
Derivatives(Optional)

Main execution file
Optimization param
Visualization

BackendOCP2NLP

IPOPT

MPCC Formulation
Collocation methods
Explicit Euler

MPCC Solver.

OutputControl & Optimization
Optimal Trajectory.
Contact Forces

ADOL-C
Automatic
Differentiation

I. I NTRODUCTION
We present a python-based robotic control and optimization
package (called P Y ROBO COP) that allows solution to a largeclass of mathematical programs with nonlinear and complementarity constraints. Contact-rich robotic manipulation tasks
could be modeled as complementarity systems. Obtaining afeasible, let alone an optimal trajectory, can be challenging
for such systems. To the best of our knowledge, none of
the existing python-based open-source optimization packages
can provide support for trajectory optimization with support
for complementarity constraints that arise from contact-richmanipulation and simple specification of obstacle avoidance
constraints. However, this is highly desirable to optimize alarge-class of contact-rich robotic systems.
We present P Y ROBO COP , a lightweight but powerful Pythonbased package for control and optimization of robotic systems.
P Y ROBO COP handles contact and collision avoidance in an
unified manner and uses ADOL-C [1] and IPOPT [5] atits backend for automatic differentiation and optimization,respectively. The main features of P Y ROBO COP are:• Contact and obstacle avoidance modeling by complementarity constraints• Support for minimum time problems• Support for optimization over fixed mode sequence problems with unknown sequence time horizons• Parameter estimation in linear complementarity systems.
Codes and instructions for installing and using P Y ROBO COP
could be found in [4] under a research only license.
II. P ROBLEM D ESCRIPTIONP Y ROBO COP solves the dynamic optimization problem
Ztf
x,y,u,pwhere x(t) ∈ Rnx , y(t) ∈ Rny , u(t) ∈ Rnu , ẋ(t) ∈ Rnx ,p ∈ Rnp are the differential, algebraic, control, time derivativeof differential variables and time-invariant parameters respectively. Each l ∈ L defines a pair of indices σl,1 , σl,2 ∈{1, . . . , ny } that specifies the complementarity constraint between the algebraic variables [y(t)]σl,1 and [y(t)]σl,2 . In (1c)νl,1 , νl,2 correspond to either the lower or upper bounds
on the corresponding algebraic variables. For example, ifthey are set respectively to the lower and upper bounds ofcorresponding algebraic variables then (1c) in combination
with the bounds (1d) model the complementarity constraint
0 ≤ [y(t) − y]σl,1 ⊥ [y − y(t)]σl,2 ≥ 0.
The dynamic optimization problem in (1) is transcripted toa NonLinear Program (NLP) using the Implicit Euler timestepping scheme. If complementarity constraints are present
then it is an instance of a Mathematical Program withComplementarity Constraints. P Y ROBO COP implements twopossible relaxation schemes for complementarity constraints
αl ([yi ]σl,1 − νl,1 )([yi ]σl,2 − νl,2 ) ≤ δ ∀ l ∈ LX
αl ([yi ]σl,1 − νl,1 )([yi ]σl,2 − νl,2 ) ≤ δ(2a)(2b)l∈L

where αl = 1 if the involved bounds (νl,1 , νl,2 ) are either bothlower or both upper bounds. More details could be found in
the extended version of the paper in [3].
III. S OFTWARE D ESCRIPTIONc(x(t), y(t), u(t), p)dt + ϕ(x(tf ), p)

minFig. 1: Workflow in P Y ROBO COP. The dynamics provided
by the user create a MPCC which is optimized using IPOPT.̇
The gradients are evaluated using automatic differentiation.

(1a)

t0

s.t. f (ẋ(t), x(t), y(t), u(t), p) = 0, x(t0 ) = xb(1b)([y(t)]σl,1 − νl,1 )([y(t)]σl,2 − νl,2 ) = 0 ∀ l ∈ L (1c)x ≤ x(t) ≤ x, y ≤ y(t) ≤ y, u ≤ u(t) ≤ u(1d)Figure 1 provides a high-level summary of the control flowin P Y ROBO COP. Detailed descriptions on the software API
and classes are available in the Software Description in [4].
A user provided class specifies the dynamic optimization
problem (1). This is also briefly described in Figure 1. The
user needs to provide the equality constraints for the dynamical

0.30
0.25
0.6

0.20

Y[m]

Y [m]

0.15
0.4

Pulleys

6 DOF robot

0.10
0.05

0.2

0.00
−0.050.0−0.2−0.2

F/T sensor
0.00.2X[m]0.40.6(a) Pushing with obstacles

P2Parallel jaw gripper

P1Belt(b) Belt Drive Unit

−0.10
−0.1

0.00.1X [m]0.20.3(c) Optimal pushing sequence opti- (d) System Identification for commizing a mode sequence.
plementarity systems

Fig. 2: Examples of contact-rich and parameter estimation problems solved with P Y ROBO COP .
system. These constraints could include the dynamics information for the system, the bounds on the system state and inputs,
and information about complementarity constraints, if any.
Furthermore, a user needs to provide the objective function.
P Y ROBO COP is interfaced with ADOL-C [1] to computederivatives (see the Backend block in Figure 1). Note that theADOL-C can also provide the sparsity pattern of the constraint
Jacobian and Hessian of the Lagrangian.
IV. N UMERICAL RESULTS
P Y ROBO COP is validated in several robotic simulations
and real world experiments. To foster reproducibility, thesource code of the following examples is available in [4] andmore details about the experiments are available in [3].
Benchmark systems. First, P Y ROBO COP was tested inmany of the standard control benchmarks with and without
contact interactions e.g., the inverted pendulum, the acrobat
system and the cart pole with softwalls.
Top-table manipulation. A second class of problems thatwas solved with P Y ROBO COP is the pushing objects ina plane class. Consider a pusher-slider system, the pusherinteracts with the slider by exerting forces in the normal andtangential direction as well as a torque about the center of themass of the object. The applied wrench causes the object tomove in a perpendicular direction to the limit surface. Moreover, consider the case when the pusher has to avoid obstacles
in order to push the slider in a determined goal position asshown in Fig. 2(a). P Y ROBO COP can handle complementarityand obstacle avoidance constraints simultaneously.
Experiments on physical devices. Third, P Y ROBO COP
solved the trajectory optimization problem of a Belt Drive Unit
as described in our previous paper [2]. The goal was to wrapwith a robotic manipulator a flexible belt around two pulleys,
see Fig. 2(b). This is a complex manipulation problem thatinvolves contact, flexible objects and collision avoidance.
Optimization with Mode Enumeration. Fourth, we show
our optimization approach over fixed mode sequences using
the pusher-slider system. When considering sticking contact
at the 4 faces of the slider, the modes appear based on whichface the pusher contacts the slider. The optimization processensures continuity of dynamics and selection of final timefor each mode in a trajectory. For this example, we use twomodes, pushing from the left face followed by pushing fromthe top face of the slider and the goal is to bring the slider toits original position rotate of 180◦ . The optimal trajectory isshown in Fig. 2(c).
System Identification for Complementarity Systems.
Fifth, the problem of parameter estimation for systems withcomplementarity constraints is a particular case of the NLP
transcription of the optimization problem (1). The objective
is to identify the physical parameters of a system given a setof collected data. As a case of study, we consider the cartpole with softwalls and the cost function is the normalized
Root Mean Square Error between the observed trajectory
and the trajectory obtained from the estimation procedure.
We validated the method with a Monte Carlo simulation ondifferent cartpole systems with increasing levels of additive
independent Gaussian noise. The results are shown in Figure 2(d) with noise standard deviation for each experiment of[0.0001, 0.001, 0.01, 0.05].
V. C ONCLUDING R EMARKSThis paper presented P Y ROBO COP , a Python-based optimization package for model-based control of robotic systems.
We showed that P Y ROBO COP can be used to solve trajectory
optimization problems of a number of dynamical systems inpresence of contact and collision avoidance constraints. Moredetails are available in the software package [4].
R EFERENCES[1] Andreas Griewank, David Juedes, and Jean Utke. Algorithm 755: Adol-c: A package for the automatic differentiation of algorithms written in c/c++. ACM Trans. Math.
Softw., June 1996. doi: 10.1145/229473.229474.
[2] S. Jin, D. Romeres, A. Ragunathan, D. Jha, andM. Tomizuka. Trajectory optimization for manipulationof deformable objects: Assembly of belt drive units. InIEEE ICRA, 2021.
[3] A. U. Raghunathan, D. K. Jha, and D. Romeres. Pyrobocop: Python-based robotic control & optimization package
for manipulation and collision avoidance, arXiv, 2021.
[4] Arvind U. Raghunathan, Devesh K. Jha, and DiegoRomeres. Pyrobocop, 2021. URL https://www.merl.com/
research/license/PyRoboCOP.
[5] A. Wächter and L. T. Biegler. On the implementation of
an interior-point filter line-search algorithm for large-scalenonlinear programming. Math. Program., 106:25–57,
2006. doi: https://doi.org/10.1007/s10107-004-0559-y.