Simultaneous Trajectory Optimization and Contact Selection for Contact-rich Manipulation with High-Fidelity Geometry

Our method requires the following information as inputs:

1. 1.

   Object initial pose region: $Q_{init}\subset SE(2)\;\text{or}\;SE(3)$.

2. 2.

   Object goal pose region: $Q_{goal}\subset SE(2)\;\text{or}\;SE(3)$.

3. 3.

   Object properties: a rigid body $\mathcal{O}$ whose geometry, mass distribution, and friction coefficients with both the environment $\mu_{env}$ and the manipulator $\mu_{mnp}$ are known.

4. 4.

   Environment properties: rigid environment $\mathcal{E}$ whose geometry is known.

5. 5.

   Robot’s contact point(s) with the object: $c^{mnp}$.

6. 6.

   A time step $\Delta t$ and number of time steps $T$ in the trajectory.

Our method will output a trajectory $\tau$ that includes the following information at time $t$:

1. 1.

   Object’s configuration: $q_{t}$.

2. 2.

   Object’s velocity (angular and translational): $v_{t}$.

3. 3.

   Robot’s contact point(s): $c^{mnp}_{t}$.

4. 4.

   Manipulation force: $u_{t}$.

5. 5.

   Object’s contact points with the environment: $\tilde{Y}_{t}$.

6. 6.

   Contact force at each object-environment contact point: $z(y_{t})\;\forall y_{t}\in\tilde{Y}_{t}$.

In this paper, we treat all objects and environments as rigid bodies and we assume the contact between the manipulator and the object is sticking contact. Furthermore, users are provided with the flexibility to opt for either quasistatic or quasidynamic assumptions based on their specific requirements. Under the quasistatic paradigm, inertial forces are considered negligible, necessitating that the object remains in a state of force and torque equilibrium at all times. Quasidynamic manipulation accounts for scenarios where tasks may involve occasional dynamic periods, during which accelerations do not integrate into substantial velocities, and both momentum and the effects of impact restitution are negligible [21].

Overall, STOCS formulates contact-rich trajectory optimization as an infinite program (IP), which is a constrained optimization with a potentially infinite set of variables and constraints. It uses an exchange method to solve the IP, by wrapping a contact selection outer loop around a finite optimization problem. The inner problem formulates CITO with complementarity constraints and solves an MPCC. In summary, the algorithm operates as follows:

1. 1.

   Initialize the initial guess object trajectory, e.g., a linear interpolation.

2. 2.

   Initialize an empty candidate set of contact points $\tilde{Y}_{1},\ldots,\tilde{Y}_{T}$ for each time step and an empty set of contact forces.

3. 3.

   Use an Oracle to identify new / removed contact points, and update $\tilde{Y}_{1},\ldots,\tilde{Y}_{T}$.

4. 4.

   Solve an MPCC for the candidate set of contact points using a small number of iterations.

5. 5.

   Check for convergence. If not converged, repeat from step 3.

Contact forces for existing contact points are maintained from iteration to iteration to warm-start the next MPCC. Details about the algorithm (Alg. 1) and oracle design are described below.

As an example, to solve pose optimization with non-penetration constraints [22], $q$ describes the pose of the object, and $y$ is a point on the surface of the object $\mathcal{O}$, where $Y\equiv\partial\mathcal{O}$ denotes the surface of $\mathcal{O}$. The constraint $g(\cdot,\cdot)$ is the signed distance from a point to the environment. Although the inequality $g(q,y)\geq 0$ must be satisfied for all $y$, optimal solutions will be supported by some points, i.e., at the optimal solution $q^{\star}$, the Karush-Kuhn-Tucker conditions will be established by a finite set of points $y$ such that $g(q^{\star},y)=0$.

In trajectory optimization, constraints need to be enforced in space-time [22], so we define a candidate index point $y_{t}\in\partial\mathcal{O}$ as being indexed by time. We denote $Y_{t}=\partial\mathcal{O}$ as the index domain at time $t$.

STOCS then reasons about contact forces at each index point, and since the contact force distribution is a function over the object surface, this introduces infinitely many variables, which turns the problem into an infinite program [23]. Specifically, we define the contact force $z(\cdot):Y_{t}\rightarrow\mathbb{R}^{r}$ as an optimization variable. Our formulation imposes friction constraints, complementarity constraints, and quasi-dynamic/quasi-static force and moment balance on these variables.

To formulate the force variables and friction cone, we encode normal and tangential forces separately in the vector $z$. In 2D, $z(y)=[z^{N},z^{+},z^{-}]$ is expressed in a reference frame with $z^{N}$ the component of force normal to the contact surface, and $z^{+}$, $z^{-}$ tangent to the contact surface. In 3D, following [5], we use a polyhedral approximation of the friction cone [24]. We express $z(y)=[z^{N},z^{D1},\cdots,z^{Dd}]$ in a reference frame with $z^{N}$ normal to the contact surface, and $z^{D1},\cdots,z^{Dd}$ tangent to the contact surface. The convex hull of the unit vectors along the directions of $z^{D1},\cdots,z^{Dd}$ in $\mathbb{R}^{2}$ forms the polyhedral approximation. The force is required to satisfy $z\geq 0$ and $\mu z^{N}-\sum_{i=D_{1}}^{D_{d}}z^{i}\geq 0$ where $\mu$ is the friction coefficient of the given point and the right hand side is the sum of tangential forces.

In summary, the constraints we impose at time $t$ are as follows:

1. 1.

   State and Control Bounds:

   $$q_{t}\in\mathcal{Q},\;v_{t}\in\mathcal{V},\;u_{t}\in\mathcal{U}$$

   (2)

2. 2.

   Object Dynamics:

   $$q_{t}-q_{t+1}+v_{t+1}\Delta t=0$$

   (3)

3. 3.

   Distance Complementarity: ensures that nonzero forces are only exerted at points where objects are in contact, i.e. the normal force $z^{N}(y)$ is nonzero only if contact is made at $y$.

   $$0\leq z^{N}(y)\perp g(q_{t},y)\geq 0\quad\forall y\in Y_{t}$$

   (4)

4. 4.

   Unilateral Manipulation Velocity: ensures the manipulator can only push the object rather than pull.

   $$c(q_{t},u_{t})\geq 0$$

   (5)

5. 5.

   Force-torque Balance:

   $$\underbrace{s_{q,u}(q_{t},u_{t})+\int_{y\in Y_{t}}s_{z}(q_{t},y,z(y))dy}_{\eqqcolon s(q_{t},u_{t},z;Y_{t})}=M\dot{v}_{t}\;\text{or}\;0$$

   (6)

6. 6.

   Friction constraints: the force constraints defined above are grouped into a function $h$

   $$h(q_{t},y,z(y))\geq 0\quad\forall y\in Y_{t}.$$

   (7)

   A similar constraint is applied to the manipulator contact force.

7. 7.

   Friction-Velocity Complementarity: ensures the relative tangential velocity at a contact is zero unless the contact force is at the boundary of the friction cone

   $$0\leq w(q_{t},v_{t},y)\perp h(q_{t},y,z(y))\geq 0\quad\forall y\in Y_{t}.$$

   (8)

We elaborate a bit on (6), which integrates the force distribution over the domain $Y_{t}$. Here, $s_{q,u}(q,u)$ represents the force and torque applied by gravity and the manipulator, and $s_{z}(q,y,z(y))$ represents the force and torque applied on an index point $y$ by the contact force $z(y)$. The integral gives the net force and torque experienced by the object, which is $0$ if we assume quasi-static, and is $M\dot{v}_{t}$ if we assume quasi-dynamic, where $M$ is the inertia matrix of $\mathcal{O}$, and $\dot{v}_{t}$ is approximated as $v_{t}/\Delta t$.

Lastly, adding terminal constraints $q_{0}\in Q_{init}$ and $q_{T}\in Q_{goal}$, we formulate the following infinite programming with complementarity constraints trajectory optimization (IPCC-TO) problem:

where $q$, $v$, and $u$ concatenate the state and control variables along all time steps, and $z$ represents the contact force distribution at all points in time. Detailed instantiations of Equation 9 for both 2D and 3D scenarios are available in the Appendix. We denote this problem as $P(Y)$.

The exchange method progressively instantiates finite index sets $\tilde{Y}\equiv[\tilde{Y}_{0},\cdots,\tilde{Y}_{T}]$ and their corresponding finite-dimensional MPCCs whose solutions converge toward the true optimum [26]. The solving process can be viewed as a bi-level optimization. In the outer loop, index points are selected by an oracle to be added to the index set $\tilde{Y}$, and then in the inner loop, the optimization $P(\tilde{Y})$ is solved. The outer loop will then decide how much should move toward the solution of $P(\tilde{Y})$. Specifically, if we let $(\tilde{x}^{*}=[q^{*},v^{*},u^{*}],\tilde{z}^{*})$ be the optimal solution to $P(\tilde{Y})$, then as $\tilde{Y}$ grows denser, the iterates of ($\tilde{x}^{*}$, $\tilde{z}^{*}$) will eventually approach an optimum of $P(Y)$.

Given a finite number of instantiated contact points $\tilde{Y}\subset Y$, we can solve a discretized version of the problem which only creates constraints and variables corresponding to $\tilde{Y}$. Also, through replacing the distribution of $z(y)$ with Dirac impulses, integrals are replaced with sums and we formulate the finite MPCC problem $P(\tilde{Y})$ in the following form:

where $\tilde{f}(q,v,u,z)\coloneqq\sum_{t=0}^{T}[f_{q,v,u}(q_{t},v_{t},u_{t})+\sum_{y\in\tilde{Y}_{t}}f_{z}(q_{t},y,z(y))]$, and $\tilde{s}(q_{t},u_{t},z;\tilde{Y}_{t})=s_{q,u}(q_{t},u_{t})+\sum_{y\in\tilde{Y}_{t}}s_{z}(q_{t},y,z(y))$.

where $b$ denotes the vector of constraint violations of Problem (9). Also, in SIP for collision geometries, a serious problem is that using existing instantiated index parameters, a step may go too far into areas where the minimum of the distance function $g^{*}(q_{t})\equiv\min_{y\in Y_{t}}g(q_{t},y)$ greatly violates the inequality, and the optimization loses reliability. So we add the max-violation $g^{*-}(q_{t})$ to $b$, in which we denote the negative component of a term as $\cdot^{-}\equiv\min(\cdot,0)$.

The convergence condition is defined as $\alpha\|[\Delta x,\Delta z]\|\leq\epsilon_{x}\cdot n_{xz}$ and $|z_{k}|^{T}|g(q_{k},\tilde{Y}^{k})|+|v(q_{k},v_{k},\tilde{Y}^{k})|^{T}|h(q_{k},\tilde{Y}^{k},z_{k})|\leq\epsilon_{gap}\cdot n_{cc}$ and $|s(x_{k},z_{k},\tilde{Y}^{k})|\leq\epsilon_{s}\cdot T$ (or  $|s(x_{k},z_{k},\tilde{Y}^{k})|-M\dot{V}\leq\epsilon_{s}\cdot T$) and $\sum_{t}g_{k}^{-*}(x_{k,t})<\epsilon_{p}\cdot T$, where $n_{xz}$ is the dimension of the optimization variable and $n_{cc}$ is the number of complementarity constraints, $\epsilon_{x}$ is the step size tolerance, $\epsilon_{gap}$ is the complementarity gap tolerance, $\epsilon_{s}$ is the balance tolerance, and $\epsilon_{p}$ is the penetration tolerance. With a little abuse of notation, $g(x_{k},\tilde{Y}^{k})$ is the concatenation of the function value of all the points in $\tilde{Y}^{k}$, and similar for $v(q_{k},v_{k},\tilde{Y}^{k})$ and $h(q_{k},\tilde{Y}^{k},z_{k})$. $\dot{V}$ is the concatenation of all the $\dot{v}_{t}$ for $t\in\mathcal{T}$.

As described above, the oracle design is a key component of STOCS. We compare Maximum Violation Oracle (MVO), that adds the closest / deepest penetrating points between the object and the environment at each time step along the trajectory, along with a new method, Time Active Maximum Violation Oracle (TAMVO) with smoothing. TAMVO selects index points more judiciously and only adds the closest / deepest penetrating points at a specific time step.

MVO (Alg. 2) adds the closest or deepest penetrating points between the object and environment at each time step along the trajectory to all candidate index points across time $\tilde{Y}_{t}^{k}$s. Note, however, that it may still include index points that do not generate active contact forces during the iteration. For instance, as illustrated in Fig. 2(a), the closest or deepest penetrating points at time step $t$ are typically active only around that specific period.

To address this issue, we introduce TAMVO (Alg. 3). In this refined approach, the index set is no longer the same across time steps. Lines 8–12 identify closest points at each time step. Duplicate points (within threshold $\epsilon$) are excluded in lines 13–18. Given default parameters $n_{t}=0$ and $N_{s}=[0]$, adds only the closest or most deeply penetrating points at the current time step $t$ to $\tilde{Y}_{t}^{k}$.

Choosing only the closest points at the current iterate is potentially not the most ideal choice unless the current iterate is near-optimal. A better choice would anticipate which points are active at the optimum. To address this, we introduce the following two strategies designed to mitigate this issue.

First, we compare STOCS with vanilla MPCC to evaluate the efficacy of dynamic contact selection on an object pivoting task. We finely discretize the object geometries to better illustrate the advantages of our method. Vanilla MPCC involves adding all index points in $Y$ to an MPCC problem without selection, resulting in a larger optimization problem than $P^{k}$ in STOCS. MVO is employed in this experiment, and $T=20$, $\Delta T=0.1\;s$, $\mu_{mnp}=1.0$ and $\mu_{env}=0.5$ are used for this set of the experiments.

The results are presented in Table I. We observe that STOCS can be around two to three orders of magnitude faster than MPCC, and can solve problems that MPCC cannot solve. STOCS selects only a small amount of points from the total number of points in the objects’ representation on average, which greatly decreases the dimension of the instantiated optimization problem and reduces solve time. The solved trajectories of STOCS for three different object are shown in 3.

Next, we evaluate STOCS in a 2D setting with TAMVO, focusing on testing its applicability with objects and environments whose geometries are significantly more complex than those presented in the examples of [20]. In this experiment, we set $n_{t}=1$ and $N_{s}=[1e^{-2}]$ as the parameters for TAMVO, and we designate these as the default values for TS and SD separately. Four tasks are designed for this evaluation: pushing a dented object across uneven terrain, inserting a tilted peg into a correspondingly angled hole, and maneuvering a bean-shaped object across two distinct curvilinear terrains. The trajectories planned for these tasks are depicted in Fig. 4, and the detailed information regarding the objects’ geometries and the solve of the trajectories are presented in Table II.

$T=10$, $\Delta T=0.1\;s$, $\mu_{mnp}=1.0$ and $\mu_{env}=0.5$ are used for this set of the experiments in 2D.

Same as before, STOCS selects only a small number of points from the entire set in the object’s representation to establish a solution. Typically, STOCS reaches convergence after just a few iterations. The final example, sliding the bean along curve 2, demands additional iterations and time for convergence due to the curve’s curvature shifting from concave to convex. This alteration results in different contact points at different stages along the trajectory, which takes more iterations to be fully instantiated.

Next, we evaluate STOCS in 3D. To demonstrate its generalizability, we collect object geometries from the YCB dataset [29], the Google Scanned Objects [30], and 3D models found online [31, 32]. All the objects used in the study are shown in Fig. 5, and all the environments used in the study are shown in Fig. 6. Klampt [33] and the code in [22] are used to find the closest points between two complex shaped geometries.

As demonstrated in the first set of experiments, employing a vanilla MPCC approach by incorporating all index points in $Y$ into an MPCC problem without selection results in a large optimization problem. Such an approach can take a long time to find a solution. Consequently, we have opted not to pursue experiments with the vanilla MPCC in this study, given that the 3D point clouds utilized herein contain thousands of points on average. Additionally, approximating the friction cone with a polyhedral model in a 3D context further increases the number of constraints for each index point, exacerbating the complexity beyond that encountered in 2D scenarios.

To demonstrate the effectiveness of STOCS in planning with high-fidelity geometric representations, we conduct experiments on ten different objects represented by dense point clouds sampled on the surface of the objects’ meshes, and five different environments represented by Signed Distance Field (SDF). The SDFs are calculated offline on a grid that encloses the corresponding environment given a closed polygonal mesh of the environment, and values off of the grid vertices are approximated via trilinear interpolation.

Using STOCS, we plan for pushing, pivoting, rolling and rotating trajectories on these objects. The resulting planned trajectories are illustrated in Fig. 9, while detailed information regarding the objects’ geometries and the solve of the trajectories are presented in Table  III. Like the experiment in 2D, we set $n_{t}=1$ and $N_{s}=[1e^{-2}]$ as the default parameters for TAMVO. $\Delta T=0.1\;s$, $\mu_{mnp}=1.0$ and $\mu_{env}=1.0$ are used for all the experiments in 3D. For all experiments, $T=10$ is used except in the tasks of pushing a basket on a shelf and sliding a plate on another plate, where $T=5$ is used. To model the robot manipulatior as having a patch contact, 3 to 5 object vertices in the neighborhood of the indicated cone are allowed to be used as contact points.

Following the initial assessments, we further evaluated the efficacy of the TAMVO alongside the SD and TS techniques through a set of comparative experiments. These experiments utilized STOCS to plan trajectories for the same set of tasks, with the primary variation being the specific oracle employed in each scenario.

Figure 7 presents the success rates of all tested Oracles. The data shows that the MVO achieves a $75\%$ success rate. In contrast, TAMVO without SD and TS exhibits worse performance than MVO; this is particularly evident in 3D scenarios where relying solely on the nearest object-to-environment point is inadequate for fulfilling the object’s balance constraints. The SD and TS techniques, introduced to address this challenge, both demonstrated enhanced performance when combined with TAMVO, surpassing the success rate of TAMVO alone. Furthermore, the integration of SD and TS with TAMVO consistently achieved successful trajectory planning for all tasks.

Figure 8 displays the average number of index points selected at each time step by the different Oracles assessed in our study, focusing on tasks that were successfully solved by all Oracles. As depicted in Fig. 8(a), the MVO selects a larger number of points than TAMVO and all its variations. Notably, TAMVO combined with SD and TS can successfully plan trajectories for all tasks while selecting fewer index points compared to MVO. These findings validate our hypothesis regarding TAMVO: an index point identified at time step $t$ is most valuable within a temporal vicinity of $t$. Furthermore, these results substantiate our rationale for introducing SD and TS, affirming that a localized exploration in both temporal and spatial dimensions offers a more efficient strategy than incorporating index points identified at distant time steps along the trajectory.

Figure 8(b) presents the solve times for STOCS employing various Oracles across all successful tasks. The results indicate that the quantity of index points selected by an Oracle does not necessarily correlate with the solve time. For instance, in the sphere rolling on plane task, TAMVO+SD+TS chooses a larger number of index points than TAMVO+TS, yet the solve time for TAMVO+SD+TS is much faster than that of TAMVO+TS. Similarly, in the case of the drug bottle rolling on plane task, it can be seen that both TAMVO+SD+TS and TAMVO+TS select a comparable number of index points, yet the solve time for TAMVO+TS is much faster than TAMVO+SD+TS. This phenomenon underscores that a larger number of index points does not invariably lead to longer solve time. Although TAMVO+SD+TS provides the best performance in terms of success rate, it is not guaranteed to give the best solve time for all different tasks.