# Trajectory Optimization for Manipulation of Deformable Objects:
Assembly of Belt Drive Units

Shiyu Jin1, Diego Romeres2, Arvind Ragunathan2, Devesh K. Jha2 and Masayoshi
Tomizuka1 1University of California, Berkeley, 2Mitsubishi Electric Research
Laboratories
<EMAIL_ADDRESS><EMAIL_ADDRESS>
###### Abstract

This paper presents a novel trajectory optimization formulation to solve the
robotic assembly of the belt drive unit. Robotic manipulations involving
contacts and deformable objects are challenging in both dynamic modeling and
trajectory planning. For modeling, variations in the belt tension and contact
forces between the belt and the pulley could dramatically change the system
dynamics. For trajectory planning, it is computationally expensive to plan
trajectories for such hybrid dynamical systems as it usually requires planning
for discrete modes separately. In this work, we formulate the belt drive unit
assembly task as a trajectory optimization problem with complementarity
constraints to avoid explicitly imposing contact mode sequences. The problem
is solved as a mathematical program with complementarity constraints (MPCC) to
obtain feasible and efficient assembly trajectories. We validate the proposed
method both in simulations with a physics engine and in real-world experiments
with a robotic manipulator.

##  I Introduction

While we have seen tremendous developments in the fields of artificial
intelligence in recent years [1, 2, 3, 4], robots can achieve only limited
autonomy during manipulation tasks [5]. One of the biggest challenges that
restricts general-purpose manipulation algorithms is contact dynamics.
Contact-rich manipulation tasks are difficult to solve from both modeling and
optimization perspectives. The manipulation problems become further
challenging when the manipulated objects are deformable. These kinds of
objects are ubiquitous in a lot of assembly problems, and yet they remain
poorly understood. In the assembly challenge competition in World Robot Summit
2018111https://worldrobotsummit.org/en/about/, assembly of a polyurethane belt
onto pulleys (see Figure 1) was one of the most challenging tasks [6]. While
there have been attempts to solve manipulation problems involving deformable
objects [7, 8, 9, 10, 11, 12], there is no general approach to it.

In this paper, we consider the problem of wrapping a belt around a two pulleys
system, considering as use case the challenge introduced in the World Robot
Summit 2018. Working with a deformable object like the belt presents several
challenges. These include: (i) infinite degrees of freedom for the belt; (ii)
contact rich manipulation; and (iii) long-horizon planning problem.

Figure 1: Belt drive unit assembly task. The robot grips a polyurethane belt
and assembles it on two pulleys, $P_{1}$ and $P_{2}$.

Optimization-based planning and control may be applied to various problems in
robotic manipulation. Given a controlled dynamical system, $\dot{x}=f(x,u)$,
trajectory optimization aims to design a finite-time input trajectory,
$u(t),\forall t\in[0,T]$, which minimizes some cost functions over the
resulting input and state trajectories [13, 14, 15]. In the belt drive unit
assembly, variations in the belt tensions and contact forces between the belt
and the pulleys result in a hybrid dynamical system. Elastic force can exist
or not, depending on whether the belt is slack or stretched. Contact forces
can exist or not, depending on whether the belt contacts the pulley or not.
Both elastic and contact forces might greatly impact the system dynamics.
Planning for such a hybrid system usually requires planning for each dynamic
system separately. There are many existing works on trajectory planning for
hybrid systems [16, 17, 18]. But the drawback is that those methods require a
task-specific mode schedule, which may bring extensive efforts in modeling and
parameter tuning.

Inspired by the work on trajectory optimization of rigid bodies through
contact [13, 15, 19, 20], we model the physics of contacts and the elastic
properties through complementarity constraints. The elastic belt is modeled
through a 3D keypoint representation. The hybrid behavior of the keypoints is
captured by the complementarity constraints. We formulate the trajectory
optimization problem as a Mathematical Program with Complementarity
Constraints (MPCC) [21]. We successfully solve the MPCC to compute feasible
and efficient trajectories to assemble the belt drive unit. Finally, we
implement the solution into the real system with a controller to track the
optimized trajectory.

The main contributions presented in this work are:

  1. 1.

Trajectory optimization formulation for deformable objects manipulation with
complementarity constraints. This provides a general-purpose, mathematical
framework to tackle these problems.

  2. 2.

Introduction of $3$D keypoint representation for deformable objects.

  3. 3.

Validation of the proposed approach through simulation as well as real
experiments.

##  II Related Work

Deformable linear (one-dimensional) object manipulation has been studied for
decades. A randomized algorithm was proposed to plan a collision-free path for
elastic objects [22]. Minimal-energy curves were applied to plan paths for
deformable linear objects in stable configurations [23]. In [24], a local
deformation model approximation method was proposed to control the soft
objects to desired shapes. The authors of [25, 26] extended the local
deformation model to the manipulation of cables. A deep neural network was
trained to manipulate a rope to target shape based on a sequence of images
[27]. However, those works do not consider the interaction between the
deformable cables and the environment. In [7], the authors proposed a strategy
to assemble a flexible beam into a rigid hole. An optimization-based
trajectory planning was utilized to assembly ring-shaped elastic objects in
[8], but the authors only validated their method in simulation. In [9], the
authors took the advantage of environmental contacts to shape deformable
linear objects by a vision-based contact detector. The authors of [10]
considered a scenario to assemble the roller chain to sprockets. Their
strategy successfully assemble the chain but lacks in generalization because
each step is engineered for the specific system. To advance the research on
robotic manipulation, the World Robot Summit 2018 proposed a competition on
assembly challenges [6]. The challenge highlighted the complexity of solving
manipulation tasks in a general manner, which still remains an open problem.

Optimization-based methods have been successfully implemented in many
trajectory planning scenarios [28, 29, 30]. [13] proposed a trajectory
optimization method for rigid bodies contacting the environment. They
formulated an MPCC to eliminate the prior mode ordering in discontinuous
dynamics due to inelastic impacts and Coulomb friction. The MPCC framework was
extended to a quadrotor with a cable-suspended payload system in [31]. The
complementarity constraint was utilized to model the limitation of a non-
stretchable cable length. Inspired by [15, 19, 20, 13, 31], we introduce
complementarity constraints to the belt drive unit assembly task to avoid the
hybrid modes selection due to elastic force in the belt and contact force
between the belt and the pulleys. We extend the keypoints representation
introduced for rigid objects, [32], to model elastic objects like the belt and
to formulate an MPCC to perform the assembly.

##  III Problem Formulation

The belt drive unit consists of two pulleys attached to a base and of a
deformable and stretchable belt as shown in Figure 1. The belt is assumed to
be composed of a homogeneous isotropic linear elastic material which is a
common assumption in mechanics. The pulleys have known geometries and can
rotate freely around the shafts axis. The base of the belt drive unit is fixed
to the workbench in a known pose. We assume that at the initial configuration,
called $\rho_{0}$, the belt is grasped and lifted by a gripper held by a
robotic manipulator, and the belt is freely hanging under the effect of
gravity, see Figure 2. The task objective is to wrap the belt around the two
pulleys as shown in Figure 1.

Figure 2: Initial belt configuration, $\rho_{0}$, with keypoints
representation. The red dots represent the “grasped” keypoint $K_{1}$ and
“opposite” keypoint $K_{2}$. The yellow dashed line shows the virtual cable,
$\mathcal{C}$, of length $L$.

Inspired by recent work [32], we introduce a 3D keypoints representation for
deformable objects. This representation consists of identifying points in the
object that are representative of the whole object. With the proposed
representation, the problem is mathematically tractable with a finite, low
dimensional state space and interpretable constraints and cost function. In
particular, we select two 3D keypoints for the belt as shown in Figure 2. The
“grasped” keypoint, $K_{1}$, corresponds to the point-mass on the belt grasped
by the robot gripper, and the “opposite” keypoint, $K_{2}$, which is the point
on the belt that is the furthest away from $K_{1}$ when the belt is in
configuration $\rho_{0}$. In the proposed keypoint representation,
configuration $\rho_{0}$ can be represented by a virtual elastic cable,
$\mathcal{C}$, that connects $K_{1}$ and $K_{2}$. The generalized coordinates
of the system can now be described as
$q=[K_{1}^{x},K_{1}^{y},K_{1}^{z},K_{2}^{x},K_{2}^{y},K_{2}^{z},K_{1}^{roll},K_{1}^{pitch},K_{1}^{yaw}]^{\top}\in\mathbb{R}^{9}$,
where $K_{1}^{x},K_{1}^{y},K_{1}^{z},K_{2}^{x},K_{2}^{y},K_{2}^{z}$ are the
Cartesian coordinates of two keypoints and
$K_{1}^{roll},K_{1}^{pitch},K_{1}^{yaw}$ are the orientation of $K_{1}$ with
reference frame shown in Figure 2. We utilize the orientation of $K_{1}$ to
express the rotation and the twist of the belt. The action space
$u=[F_{x},F_{y},F_{z},M_{x},M_{y},M_{z}]^{\top}\in\mathbb{R}^{6}$ is the
vector of forces and torques that are applied to $K_{1}$ through the gripper.
This makes the belt drive unit an underactuated system as we cannot directly
control $K_{2}$. Finally, we assume that in configuration $\rho_{0}$ the
ellipsoidal shape of the belt is large enough to go around the first pulley
$P_{1}$.

###  III-A Subtasks Decomposition

Belt drive unit assembly is a complex task that requires a long-horizon
planner. As often proposed in the literature, long-horizon planning tasks are
decomposed into subtasks to reduce complexity. The belt drive unit assembly
can have highly engineered solutions with a dense sequence of subtasks and
simple planners whose subgoals are trivial to reach. However, this kind of
approach requires extensive effort in parameter tuning and engineering work
and lacks generality, since the goals of the subtasks need to be redefined as
the scenario changes. We partially address this problem by reducing the number
of subtasks to two. Following a logic similar to a human’s approach, the first
subtask, $S_{1}$, corresponds to wrap the belt around the first pulley, and
the second subtask, $S_{2}$, corresponds to wrap the second pulley keeping the
belt taut to maintain the wrap around the first pulley, see Figure 3. In a
qualitative description, in $S_{1}$, the belt has to avoid the outer surface
of the first pulley $P_{1}$ and $K_{2}$ has to get into the groove creating a
contact force, while $K_{1}$ is stretched until the belt is taut. In $S_{2}$,
the belt is assembled on the second pulley $P_{2}$ by rotation around the
first pulley. During the rotation, the belt should remain taut in order to
remain in the groove of the first pulley. Finally, the belt has to hook the
internal groove of $P_{2}$ and $K_{1}$ has to reach the bottom of the second
pulley to accomplish the task.

Figure 3: Two subtasks decomposition. $P_{1}$ and $P_{2}$ are two pulleys. The
blue lines represent the belt gripped at keypoint $K_{1}$ by a robot. $S_{1}$:
The belt wraps the first pulley $P_{1}$ and it stretched. $S_{2}$: The belt
rotates around the first pulley and is assembled onto the second pulley
$P_{2}$.

Given the proposed 3D keypoint representation of the belt drive unit, we can
formulate a trajectory optimization problem, that uses complementarity
constraints to model the contacts and the deformation of the belt, to solve
each of the two subtasks. The two optimized trajectories are then executed
sequentially in order to accomplish the task, the final condition of $S_{1}$
corresponds to the initial condition of $S_{2}$.

##  IV Trajectory Optimization for the Belt Drive Unit Assembly

We approach the belt drive unit assembly as a trajectory optimization problem
formulated as an MPCC. The complexity of this problem is given by the presence
of hybrid nonlinear dynamics due to contacts that may happen between the
pulleys and the belt, the elastic properties of the belt, the obstacle
avoidance constraints, and the long planning horizon. A trajectory
optimization problem is solved for each of the two subtasks described in Sec.
III-A of the form

$\displaystyle\min\limits_{q,\dot{q},u,\lambda}\quad$
$\displaystyle\,L(q,\dot{q},u,\lambda)$ (1a) s.t.
$\displaystyle\,H(q)\ddot{q}+C(q,\dot{q})+G(q)=B(q)u+\lambda$ (1b)
$\displaystyle\,g(q,\dot{q},u,\lambda)\leq 0$ (1c)
$\displaystyle\,\underline{q}\leq
q\leq\overline{q},\,\underline{\dot{q}}\leq\dot{q}\leq\overline{\dot{q}},\,\underline{u}\leq
u\leq\overline{u},\,\underline{\lambda}\leq\lambda\leq\overline{\lambda}$ (1d)

where $L(q,\dot{q},u,\lambda)$ is the cost function, $q\in\mathbb{R}^{9}$ are
the generalized coordinates described in Sec. III, $\dot{q}$ and $\ddot{q}$
are its first and second order time derivatives, $u\in\mathbb{R}^{6}$ is the
control input and $\lambda$ are the external forces acting on the belt. Eq.
(1b) is the forward dynamics, where $H(q),C(q,\dot{q}),G(q)$ are the inertial
matrix, the Coriolis terms, and the gravitational forces, respectively. $B(q)$
is input mapping. The general nonlinear constraints (1c) include the
complementarity constraints and collision avoidance. Eq. (1d) represents the
lower and upper bounds of the optimization variables.

We solve (1) as a nonlinear program and use a direct approach which in general
has better numerical properties than shooting methods, and we can exploit the
sparsity structure of the problem. We directly optimize the feasible general
coordinates and its first-time derivative, the control inputs, and the
external forces. The discretization of the forward dynamics is obtained by the
trapezoidal rule. The formulation of the contact and elastic forces as
complementarity constraints fits naturally well in this formulation. In
practice, for numerical advantages, we use a relaxed version of the
complementarity constraints as described in [33].

In the following, the dynamical constraints and the cost function for MPCC (1)
are described for the two subtasks.

###  IV-A Dynamics Constraints

The system is composed of the two keypoints, $K_{1}$ and $K_{2}$, and the
virtual elastic cable, $\mathcal{C}$. It is modeled similarly to a mechanical
mass-spring-damper second-order system, with an actuator acting on $K_{1}$ and
subject to the gravity and the external forces given by the elastic force
$\lambda_{0}$ and the normal force $\lambda_{1}$ experienced during contacts.
Figure 4 shows a schematic example of the forces that act on the system at the
end of subtask $S_{1}$.

Figure 4: Force analysis at the two keypoints. $F_{u}$ is the control input
force. $\lambda_{0}$ is the elastic force. $\lambda_{1}$ is the contact normal
force. $g$ is the gravity force.

The system dynamics are defined as

$\displaystyle\dot{x}$ $\displaystyle=Ax+Bu+G+f(x,\lambda)$ (2)

where $x=[q^{\top},\dot{q}^{\top}]^{\top}\in\mathbb{R}^{18}$ is the system
state and $\lambda=[\lambda_{0}^{\top},\lambda_{1}^{\top}]^{\top}$ is the
vector of the external forces. The state transition matrix

$A=\begin{bmatrix}0_{9\times 9}&I_{9\times 9}\\\ 0_{3\times
9}&-\frac{k_{d}}{m_{1}}I_{3\times 3}\quad\frac{k_{d}}{m_{1}}I_{3\times 3}\quad
0_{3\times 3}\\\ 0_{3\times 9}&\frac{k_{d}}{m_{2}}I_{3\times
3}\quad-\frac{k_{d}}{m_{2}}I_{3\times 3}\quad 0_{3\times 3}\\\ 0_{3\times
9}&0_{3\times 9}\\\ \end{bmatrix}\in\mathbb{R}^{18\times 18}$

represents the effect of the mass-spring-damper system with $k_{d}$ the
damping coefficient and $m_{1}$, $m_{2}$ are the masses of the keypoints
$K_{1}$ and $K_{2}$, respectively. The input matrix

$B=\begin{bmatrix}0_{9\times 6}\\\ \frac{I_{3\times 3}}{m_{1}}\quad 0_{3\times
3}\\\ 0_{3\times 6}\\\ 0_{3\times 3}\quad\frac{I_{3\times
3}}{M_{1}}\end{bmatrix}\in\mathbb{R}^{18\times 6}$

maps the 6-dimensional end-effector force/torque input $u$ to the linear and
angular acceleration of the “grasped” keypoint $K_{1}$. $M_{1}$ is the moment
of inertia of $K_{1}$. The gravitational acceleration is applied to the two
keypoints through the vector $G=\begin{bmatrix}0_{1\times
11},\,-g,\,0_{1\times 2},\,-g,\,0_{1\times
3}\end{bmatrix}^{\top}\in\mathbb{R}^{18\times 1}$.

The contribution of the external forces is now given by the sum of the elastic
and normal force $f(x,\lambda)=\lambda_{0}+\lambda_{1}$. The elastic force is
defined as

$\lambda_{0}=\begin{bmatrix}0_{3\times 9},\,-\frac{I_{3\times
3}}{m_{1}},\,\frac{I_{3\times 3}}{m_{2}},\,0_{3\times
3}\end{bmatrix}^{\top}\Pi_{K_{1},p}\,\bar{\lambda}_{0}\in\mathbb{R}^{18\times
1}$

where $\bar{\lambda}_{0}\in\mathbb{R}$ is the magnitude of the elastic force
and is the variable optimized,
$\Pi_{K_{1},p}=\frac{[(K_{1}^{x}-p^{x}),(K_{1}^{y}-p^{y}),(K_{1}^{z}-p^{z})]^{\top}}{\lvert\lvert
K_{1}-p\rvert\rvert}$ is the projection operator of the elastic force into the
3 axis. The point $p$ is $K_{2}$ in $S_{1}$ and $O_{1}$ in $S_{2}$ for
simplicity of computation. $O_{1}$ is the position of the first pulley’s
center. The normal force due to the contacts between the pulley and the
keypoint $K_{2}$ is defined as

$\lambda_{1}=\begin{bmatrix}0_{3\times 12},\,-\frac{I_{3\times
3}}{m_{2}},\,0_{3\times
3}\end{bmatrix}^{\top}\Pi_{O_{1},K_{2}}\bar{\lambda}_{1}\in\mathbb{R}^{18\times
1}$

where $\bar{\lambda}_{1}\in\mathbb{R}$ is the magnitude of the normal force
and is the variable optimized and
$\Pi_{O_{1},K_{2}}=\frac{[(o_{1}^{x}-K_{2}^{x}),(o_{1}^{y}-K_{2}^{y}),(o_{1}^{z}-K_{2}^{z})]^{\top}}{\lvert\lvert
o_{1}-K_{2}\rvert\rvert}$ is the projection operator of the normal force into
the 3-axis.

###  IV-B Complementarity Constraints

In order to model the hybrid dynamics due to elastic force and contact force,
we use complementarity constraints

$\displaystyle 0\leq g(\cdot)\quad\perp\quad h(\cdot)\geq 0$ (3)

Complementary constraints are a way to model constraints that are
combinatorial in nature and impose the positivity and orthogonality of the
variables.

Elastic force constraint. The first complementarity constraint is formulated
as

$\displaystyle\lambda_{2}=\frac{\bar{\lambda}_{0}}{k_{p}}+L-l(x)$
$\displaystyle\geq 0$ (4a) $\displaystyle\bar{\lambda}_{0}$ $\displaystyle\geq
0$ (4b) $\displaystyle\bar{\lambda}_{0}\lambda_{2}$ $\displaystyle=0$ (4c)

where $L$ and $k_{p}$ are respectively the length at configuration $\rho_{0}$
and the stiffness coefficient of the virtual elastic cable, $\mathcal{C}$. The
length of $\mathcal{C}$ at each temporal instant is $l(x)=\lvert\lvert
K_{1}-K_{2}\rvert\rvert$ in $S_{1}$, and $l(x)=\lvert\lvert
K_{1}-O_{1}\rvert\rvert+r_{1}$ in $S_{2}$, where $r_{1}$ denotes the radius of
$P_{1}$. The pulley center $O_{1}$ is chosen because it is a fixed known point
while the pulley is rotating. From eq. (4a) the elasticity of the belt is
defined as proportional to the length $L-l(x)$ and depends on the stiffness
coefficient $k_{p}$. $\lambda_{2}$ is an algebraic variable. If the cable is
stretched, then $L<l(x)$, $\bar{\lambda}_{0}>0$, and $\lambda_{2}=0$. If the
cable is slack, then $L>l(x)$, $\bar{\lambda}_{0}=0$, and $\lambda_{2}>0$.

Contact force constraint. The second complementarity constraint is formulated
as

$\displaystyle\lambda_{3}=\sqrt{\lvert\lvert
K_{2}-O_{e}\rvert\rvert^{2}+\epsilon^{2}}$ $\displaystyle\geq\epsilon$ (5a)
$\displaystyle\bar{\lambda}_{1}$ $\displaystyle\geq 0$ (5b)
$\displaystyle\bar{\lambda}_{1}\lambda_{3}$ $\displaystyle=0$ (5c)

where $O_{e}$ is the contact point on the edge of $P_{1}$. $\epsilon$ denotes
a small number to relax the complementarity constraint [33]. $\lambda_{3}$ is
the algebraic variable describes whether the belt contacts the pulley. If the
belt contacts the pulley, then $\lambda_{3}=\epsilon$, and contact force
$\bar{\lambda}_{1}\geq 0$. If there is no contact, then
$\lambda_{3}>\epsilon$, and contact force $\bar{\lambda}_{1}=0$.

###  IV-C Obstacle avoidance

This constraint imposes that the keypoints cannot penetrate into the pulleys.
Each pulley is approximated with an ellipsoid, since there is a known
analytical expression of the distance function between a point and an
ellipsoid. The obstacle avoidance constraints between a keypoint $K_{i}$ and a
pulley $P_{j}$ can be denoted as
$distance(K_{i},P_{j})=\sqrt{(K_{i}-O_{j})^{\top}S(K_{i}-O_{j})}-1\geq 0$,
where $S=diag\\{1/{a^{2}},1/{b^{2}},1/{c^{2}}\\}$ is a diagonal matrix,
$a,b,c$ are half the length of the principal axes. $O_{j}$ denotes the center
of pulley $P_{j}$.

###  IV-D Physics Limitation

The belt might break if stretched over a certain limit, this condition is
approximated by constraining the length of the virtual cable $\mathcal{C}$,
$l(x)\leq L_{max}$. Moreover, $L_{max}$ is assumed large enough for the loop
to go around two pulleys.

###  IV-E Cost Function

We use a common quadratic cost function that penalizes the difference to the
goal state $x^{goal}$ and the control input $u(k)$:

$\displaystyle J(x,u,\lambda)=\sum_{k=0}^{N}$
$\displaystyle(x(k)-x^{goal})^{\top}Q(x(k)-x^{goal})+$ (6) $\displaystyle
u(k)^{T}Ru(k)+w(\bar{\lambda}_{0}(k)-\bar{\lambda}_{0}^{desired})^{2}$

where the weights $Q$ and $R$ are diagonal matrices and $w$ is a scalar.
Moreover, the term $w(\bar{\lambda}_{0}(k)-\bar{\lambda}_{0}^{desired})^{2}$
adds a soft constraint in the elastic force. A positive
$\bar{\lambda}_{0}^{desired}$ encourages a solution with the belt in tension.
This constraint is used in subtask $S_{2}$ to maintain the belt taut. Instead,
in $S_{1}$ we set $w=0$.

##  V Experimental Results

In this section, we present the results of the proposed method both in
simulation using the physic engine MuJoCo [34] and in a real system with a
$6$-DoF manipulator. We use the Ipopt [35] solver in a python wrapper.

###  V-A Simulations

####  V-A1 Simulation Setup

The belt drive unit is represented in a simulated environment in MuJoCo as
shown in the top left corner of Fig. 5. The environment includes two pulleys
and one belt. The radius of the pulleys are of $30[mm]$ for $P_{1}$ and
$15[mm]$ for $P_{2}$. The belt is composed of 41 linked objects called
capsules in MuJoCo. Any two adjacent capsules are connected by two hinge
joints and one prismatic joint. The physical properties of the simulated belt
are tuned to resemble the belt of the real belt drive unit. The belt is held
by a parallel gripper attached to a 6 DOF Fanuc LR-Mate 200iD. The purpose of
the manipulator is to actuate the end-effector in order to track the optimal
trajectory, but in simulation could be removed.

####  V-A2 Trajectory Optimization in Different Scenarios

Figure 5: Snapshots of $4$ different simulation scenarios at the goal position
of subtask $S_{2}$. The relative positions of the two pulleys vary in each
scenario.

In order to verify the generality of the approach to different known
geometries, we consider 4 different scenarios, where the position of $P_{1}$
is fixed and the position of the smaller pulley $P_{2}$ varies, see Figure 5
and Table I. Each pulley is modeled as three adjacent cylinders, and the two
outer cylinders have larger radius than the inner one. The belts’ lengths,
$P_{belt}$, are chosen in each scenario based on the distance between two
pulleys. The mass of the keypoints is $m_{1}=m_{2}=0.042[kg]$. The moment of
inertia of $K_{1}$ is $M_{1}=10^{-7}[kgm^{2}]$. The belt’s stiffness and
damping coefficient are $k_{p}=63.34[N/m]$ and $k_{d}=4.65[Ns/m]$,
respectively.

In the trajectory optimization formulation described in Section IV, the goal
for subtask $S_{1}$ is set vertically above the pulley $P_{1}$ for keypoint
$K_{1}$, and right under the pulley $P_{1}$ for keypoint $K_{2}$,
respectively, e.g.,
$q_{1}^{goal}=~{}[0.10,0.55,0.53,0.10,0.23,0.34,0,0,0]^{T}$ and both with zero
velocity. In this substask there is a change of mode in the dynamics from no
contact to contact between the belt and the environment and the deformation of
the belt for reaching the desired target.

In the second subtask $S_{2}$, the goal, $q_{2}^{goal}$, is set only for
$K_{1}$ in both the Cartesian coordinates and angular orientation, according
to the position of the pulley, $P_{2}$. A qualitative representation of the
goal position is shown in Figure 5 for each of the scenarios. The desired
$[K_{1}^{roll},K_{1}^{pitch},K_{1}^{yaw}]^{\top}$ is
$[-\pi/2,0,\pi/2]^{\top}$. The twist of the virtual cable $\mathcal{C}$
approximates the twist of the belt which leads to the assemble onto the groove
of the pulley $P_{2}$. Based on $q_{2}^{goal}$ and $k_{p}$ it is possible to
compute the target elastic force as $\bar{\lambda}_{0}^{desired}$, which
encourages the belt to be stretched during rotation.

TABLE I: Simulation results in 4 scenarios. In each scenario the position of the pulley center $O_{2}$ varies. | $P_{belt}$ |  $O_{1}$ (center of $P_{1}$) |  $O_{2}$ (center of $P_{2}$) | Feasible trajectory | Successful assembly
---|---|---|---|---|---
Scenario 1 | $0.4m$ | [0.100, 0.550, 0.340] | [0.100, 0.680, 0.340] | 10/10 | 10/10
Scenario 2 | $0.4m$ | [0.100, 0.550, 0.340] | [0.100, 0.642, 0.432] | 10/10 | 8/10
Scenario 3 | $0.4m$ | [0.100, 0.550, 0.340] | [0.100, 0.645, 0.275] | 10/10 | 7/10
Scenario 4 | $0.6m$ | [0.100, 0.550, 0.340] | [0.100, 0.780, 0.340] | 10/10 | 9/10

We perform 10 experiments for each scenario. In each experiment, the goal
positions of the “grasped” keypoint $K_{1}$ are sampled from the normal
distributions $\mathcal{N}(\mu_{1},\,\Sigma)$ in $S_{1}$ and
$\mathcal{N}(\mu_{2},\,\Sigma)$ in $S_{2}$. Where,
$\mu_{1},\,\mu_{2}\in\mathbb{R}^{3}$ are the components
$[K_{1}^{x},K_{1}^{y},K_{1}^{z}]^{\top}$ in a pre-selected successful
$q_{1}^{goal}$ and $q_{2}^{goal}$ and
$\Sigma~{}=~{}diag\\{0.005,0.005,0.005\\}$. The lower and upper constraints of
position, velocity, tilt angle, and force are $\pm 1m$, $\pm 0.5m/s$,
$\pm\pi$, and $\pm 50N$, respectively.

####  V-A3 Results

Figure 6: Trajectory of keypoint $K_{1}$ in a successful assembly for scenario
Figure 5(3). The dashed line is the reference trajectory obtained from MPCC.
The solid line is the measured trajectory.

The simulation results are shown in Table I. We initialize the trajectory with
all states $x(k)=x(0)$, where $k=0,1,..,N$. The solver finds a feasible
trajectory for both subtasks given any sampled goals. The optimal trajectory
obtained for $K_{1}$ is then tracked by the end-effector, and the assembly is
completed successfully in $34/40$ experiments. The failure cases happen when
the goal is sampled away from $q_{1}^{goal}$ or $q_{2}^{goal}$ because the
belt fails to wrap around the pulley. The purpose of these experiments is to
show that the engineering effort in finding the goal position for the subtask
is reduced as it is not required to provide one specific point. But also the
trade-off of having only two keypoints, more keypoints would make a more
accurate model but also a more complex optimization problem. We use an Intel
12 Cores i7-9850H CPU @ 2.60GHz. The average computational time for one
trajectory with $600$ time steps is $36.138\pm 5.747[s]$. The computational
time highly depends on the number of time steps selected. Figure 6 shows one
full successful assembly trajectory for scenario Figure 5(3).

###  V-B Real-World Experiments

Figure 7: Snapshots of the experiment.

####  V-B1 Experimental Setup

As shown in Figure 1, the experiment environment includes a 6 DOF FANUC LR-
Mate 200iD, an ATI Mini45 F/T sensor, and a 3D printed belt drive unit of the
same dimensions in the assembly challenge [6] fixed on a vise. The belt is the
same as in the challenge with length $0.40[m]$ and is gripped by a parallel
jaw gripper. We assume no slip between the belt and the robot gripper. The
pose of the pulleys is known exactly.

####  V-B2 Results

Figure 8: Forces and positions of the end-effector in a successful experiment.
The red circle and square represent the end of subtask $S_{1}$, $S_{2}$,
respectively.

Figure 7 provides the snapshots of the main phases during the execution of a
successful experiment. Figure 8 shows the trajectory of the gripper tip that
corresponds to $K_{1}$ and the measured forces at the robot’s wrist along the
trajectory. In the beginning, (Figure 7a), the belt approaches the pulley and
position $X$ increases and the forces are zero. The position $Z$ goes down at
$5.57[s]$ to avoid the outer cylinder of the first pulley. At $6.29[s]$,
position $X$ stops increasing because the pulley is reached (Figure 7b). Then
the $Z$ position increases as the belt is lifted and contacts the pulley at
$7.82[s]$ with a corresponding increase in force along the negative direction
in $Z$. At $10.50[s]$, the system accomplishes $S_{1}$ (Figure 7c). After
that, the belt is rotated around $O_{1}$ (the $Z$ position decreases, and $Y$
position increases) while being stretched (Figure 7d). In this phase, the
measured net force is closed to the desired elastic force
$\bar{\lambda}_{0}^{desired}$. The target orientations are
$[K_{1}^{roll},K_{1}^{pitch},K_{1}^{yaw}]^{\top}=[-\pi/2,0,\pi/4]^{\top}$. The
belt is twisted so that it hooks the second pulley without jamming. Finally,
the goal of subtask $S_{2}$ is reached at $29.00[s]$ (Figure 7e) and the
gripper releases the belt (Figure 7f).

The experiment has been repeated multiple times but given the robot’s accuracy
the results were similar to each other.

##  VI Conclusion

In this paper, we propose a trajectory optimization formulation to assemble
the belt drive unit. We propose a 3D keypoints representation to model the
elastic belt, which simplifies the complexity of the trajectory optimization
problem. The problem is formulated as an MPCC with complementarity constraints
to model the hybrid dynamics due to contact and elastic forces. Simulations
results show that the proposed approach can find feasible trajectories for the
belt drive unit assembly with known but variable geometry. To the best of our
knowledge, this is the first work that formalizes the trajectory optimization
problem for the belt drive unit assembly, and the solution works in the real
system. Several future works are possible. The current method is based on the
execution of an open-loop trajectory which could fail under uncertainties in
the position of the pulleys or of the belt. Adding a feedback controller is
fundamental for a more robust and reliable solution. Moreover, in order to
improve the generality of the problem, we are interested in an autonomous
selection of the 3D keypoints for a given task. Our formulation of a
trajectory optimization problem for deformable objects using complementarity
constraints is not limited to belt drive unit assembly. The proposed method
might be applied to a wider range of tasks such as cable routing and wire
harness.

## References

  * [1] Y. LeCun, Y. Bengio, and G. Hinton, “Deep learning,” _nature_ , vol. 521, no. 7553, pp. 436–444, 2015.
  * [2] A. Krizhevsky, I. Sutskever, and G. E. Hinton, “Imagenet classification with deep convolutional neural networks,” _Communications of the ACM_ , vol. 60, no. 6, pp. 84–90, 2017.
  * [3] D. Silver, J. Schrittwieser, K. Simonyan, I. Antonoglou, A. Huang, A. Guez, T. Hubert, L. Baker, M. Lai, A. Bolton _et al._ , “Mastering the game of go without human knowledge,” _nature_ , vol. 550, no. 7676, pp. 354–359, 2017.
  * [4] S. Jin, X. Zhu, C. Wang, and M. Tomizuka, “Contact pose identification for peg-in-hole assembly under uncertainties,” _arXiv preprint arXiv:2101.12467_ , 2021.
  * [5] M. T. Mason, “Toward robotic manipulation,” _Annual Review of Control, Robotics, and Autonomous Systems_ , vol. 1, pp. 1–28, 2018.
  * [6] F. von Drigalski, C. Schlette, M. Rudorfer, N. Correll, J. Triyonoputro, W. Wan, T. Tsuji, and T. Watanabe, “Robots assembling machines: Learning from the world robot summit 2018 assembly challenge,” 2019.
  * [7] Y. F. Zheng, R. Pei, and C. Chen, “Strategies for automatic assembly of deformable objects,” in _Proceedings. 1991 IEEE International Conference on Robotics and Automation_ , 1991, pp. 2598–2603 vol.3.
  * [8] I. G. Ramirez-Alpizar, K. Harada, and E. Yoshida, “Motion planning for dual-arm assembly of ring-shaped elastic objects,” in _2014 IEEE-RAS International Conference on Humanoid Robots_ , 2014, pp. 594–600.
  * [9] J. Zhu, B. Navarro, R. Passama, P. Fraisse, A. Crosnier, and A. Cherubini, “Robotic manipulation planning for shaping deformable linear objects with environmental contacts,” _IEEE Robotics and Automation Letters_ , vol. 5, no. 1, pp. 16–23, 2020.
  * [10] K. Tatemura and H. Dobashi, “Strategy for roller chain assembly with parallel jaw gripper,” _IEEE Robotics and Automation Letters_ , vol. 5, no. 2, pp. 2435–2442, 2020.
  * [11] T. Tang, C. Wang, and M. Tomizuka, “A framework for manipulating deformable linear objects by coherent point drift,” _IEEE Robotics and Automation Letters_ , vol. 3, no. 4, pp. 3426–3433, 2018.
  * [12] S. Jin, C. Wang, X. Zhu, T. Tang, and M. Tomizuka, “Real-time state estimation of deformable objects with dynamical simulation,” in _Workshop on Robotic Manipulation of Deformable Objects 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , 2020.
  * [13] 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.
  * [14] P. Kolaric, D. Jha, A. Raghunathan, F. Lewis, M. Benosman, D. Romeres, and D. Nikovski, “Local policy optimization for trajectory-centric reinforcement learning,” in _IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2020, pp. 5094–5100.
  * [15] K. Yunt and C. Glocker, “Trajectory optimization of mechanical hybrid systems using sumt,” in _9th IEEE International Workshop on Advanced Motion Control, 2006._ IEEE, 2006, pp. 665–671.
  * [16] R. Goebel, R. G. Sanfelice, and A. R. Teel, “Hybrid dynamical systems,” _IEEE Control Systems Magazine_ , vol. 29, no. 2, pp. 28–93, 2009.
  * [17] R. Fierro, A. K. Das, V. Kumar, and J. P. Ostrowski, “Hybrid control of formations of robots,” in _Proceedings 2001 ICRA. IEEE International Conference on Robotics and Automation (Cat. No.01CH37164)_ , vol. 1, 2001, pp. 157–162 vol.1.
  * [18] L. P. Kaelbling and T. Lozano-Pérez, “Hierarchical task and motion planning in the now,” in _2011 IEEE International Conference on Robotics and Automation_ , 2011, pp. 1470–1477.
  * [19] K. Yunt and C. Glocker, “A combined continuation and penalty method for the determination of optimal hybrid mechanical trajectories,” in _IUTAM Symposium on Dynamics and Control of Nonlinear Systems with Uncertainty_. Springer, 2007, pp. 187–196.
  * [20] K. Yunt, “An augmented lagrangian based shooting method for the optimal trajectory generation of switching lagrangian systems,” _Dynamics of Continuous, Discrete and Impulsive Systems Series B: Applications and Algorithms_ , vol. 18, no. 5, pp. 615–645, 2011.
  * [21] Z.-Q. Luo, J.-S. Pang, and D. Ralph, _Mathematical programs with equilibrium constraints_. Cambridge University Press, 2008.
  * [22] F. Lamiraux and L. E. Kavraki, “Planning paths for elastic objects under manipulation constraints,” _The International Journal of Robotics Research_ , vol. 20, no. 3, pp. 188–208, 2001.
  * [23] M. Moll and L. E. Kavraki, “Path planning for deformable linear objects,” _IEEE Transactions on Robotics_ , vol. 22, no. 4, pp. 625–636, 2006.
  * [24] D. Navarro-Alarcón, Y. Liu, J. G. Romero, and P. Li, “Model-free visually servoed deformation control of elastic objects by robot manipulators,” _IEEE Transactions on Robotics_ , vol. 29, no. 6, pp. 1457–1468, 2013.
  * [25] J. Zhu, B. Navarro, P. Fraisse, A. Crosnier, and A. Cherubini, “Dual-arm robotic manipulation of flexible cables,” in _2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , 2018, pp. 479–484.
  * [26] S. Jin, C. Wang, and M. Tomizuka, “Robust deformation model approximation for robotic cable manipulation,” in _2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , 2019, pp. 6586–6593.
  * [27] A. Nair, D. Chen, P. Agrawal, P. Isola, P. Abbeel, J. Malik, and S. Levine, “Combining self-supervised learning and imitation for vision-based rope manipulation,” 03 2017.
  * [28] N. Ratliff, M. Zucker, J. A. Bagnell, and S. Srinivasa, “Chomp: Gradient optimization techniques for efficient motion planning,” in _2009 IEEE International Conference on Robotics and Automation_ , 2009, pp. 489–494.
  * [29] M. Kalakrishnan, S. Chitta, E. Theodorou, P. Pastor, and S. Schaal, “Stomp: Stochastic trajectory optimization for motion planning,” in _2011 IEEE International Conference on Robotics and Automation_ , 2011, pp. 4569–4574.
  * [30] J. Schulman, Y. Duan, J. Ho, A. Lee, I. Awwal, H. Bradlow, J. Pan, S. Patil, K. Goldberg, and P. Abbeel, “Motion planning with sequential convex optimization and convex collision checking,” _The International Journal of Robotics Research_ , vol. 33, no. 9, pp. 1251–1270, 2014.
  * [31] P. Foehn, D. Falanga, N. Kuppuswamy, R. Tedrake, and D. Scaramuzza, “Fast trajectory optimization for agile quadrotor maneuvers with a cable-suspended payload.” _RSS_ , 2017.
  * [32] L. Manuelli, W. Gao, P. R. Florence, and R. Tedrake, “kpam: Keypoint affordances for category-level robotic manipulation,” 2019.
  * [33] A. U. Raghunathan and L. T. Biegler, “An interior point method for mathematical programs with complementarity constraints (mpccs),” _SIAM Journal on Optimization_ , vol. 15, no. 3, pp. 720–750, 2005.
  * [34] E. Todorov, T. Erez, and Y. Tassa, “Mujoco: A physics engine for model-based control,” in _2012 IEEE/RSJ International Conference on Intelligent Robots and Systems_ , 2012, pp. 5026–5033.
  * [35] A. Wächter and L. T. Biegler, “On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming,” _Math. Program._ , vol. 106, no. 1, p. 25–57, Mar. 2006.