A General Formulation for Path Constrained Time-Optimized Trajectory Planning with Environmental and Object Contacts

TL;DR

The paper addresses time-optimal trajectory planning for robotic manipulation with grasping under full contact dynamics, including object-environment interactions. It develops a convex second-order cone program (SOCP) formulation that enforces nonlinear friction cone constraints at hand-object and environment-object contacts while respecting robot dynamics and actuator limits, using a path-parameterization through a scalar coordinate $s$ and a convex reformulation with variables $a(s)=\ddot{s}$ and $b(s)=\dot{s}^2$ where $b'(s)=2a(s)$. By performing direct transcription and introducing auxiliary variables to linearize the time-scaling term, the method yields a tractable SOCP that outputs the time scaling, joint torques, and contact forces for time-optimal execution. The approach supports multiple manipulators and multiple contact points and is demonstrated on pivoting, pickup, and non-prehensile waiter tasks, showing both feasibility boundaries and computation times on standard hardware. This work advances practical, contact-aware time-optimal manipulation by bridging geometric path planning with dynamic, contact-constrained optimization, enabling faster and safer robotic manipulation in constrained environments.

Abstract

A typical manipulation task consists of a manipulator equipped with a gripper to grasp and move an object with constraints on the motion of the hand-held object, which may be due to the nature of the task itself or from object-environment contacts. In this paper, we study the problem of computing joint torques and grasping forces for time-optimal motion of an object, while ensuring that the grasp is not lost and any constraints on the motion of the object, either due to dynamics, environment contact, or no-slip requirements, are also satisfied. We present a second-order cone program (SOCP) formulation of the time-optimal trajectory planning problem that considers nonlinear friction cone constraints at the hand-object and object-environment contacts. Since SOCPs are convex optimization problems that can be solved optimally in polynomial time using interior point methods, we can solve the trajectory optimization problem efficiently. We present simulation results on three examples, including a non-prehensile manipulation task, which shows the generality and effectiveness of our approach.

Figures (6)

• Figure 1: Example pivoting task, which is a path constrained robot task involving manipulator object and object environment contacts. Our method takes into account all the kinematic and dynamic constraints of the system to compute the time-optimal trajectory. The $(s,\dot{s})$ phase plane, which connects the initial path position to the final position, illustrates our problem. The purple curve is the time scaling that we achieve while respecting position, velocity, and acceleration constraints. The blue dots denote the maximum feasible scaling possible at the collocation points. The vertical dashed lines represent specific instances when joints $6,7,$ and $4$ are about to hit the limits. Note that we consider the full non-linear friction cone constraints and the soft finger contact with elliptic approximations for modeling the two types of contacts.
• Figure 2: A rigid body being manipulated with $v$ manipulators and in contact with the environment at $u$ points. The contacts between object and the environment are modeled as PCWF and contacts between manipulators and the objects are assumed to be SFCE. $\{E\}$, $\{M\}$, and $\{W\}$ represent the environment, manipulator, and world reference frames.
• Figure 3: Plots of joint velocities, accelerations and torques for the task of picking up an object. The joint limits are shown using dashed red lines. Joints $3$ and $4$ are the ones getting closest to their velocity and acceleration limits.
• Figure 4: Plots of joint velocities for the pivoting task. The vertical dashed lines denote specific instances when the joints are at their limits and they correspond to the vertical dashed lines shown in the $(s,\dot{s})$ phase plane in Fig \ref{['fig:cover_pic']}.
• Figure 5: Tray-Cube Friction Cone Constraint results obtained for the non-prehensile task. First three images on the left show the constraints when the tray angle was set to $10^\circ$. The next three images show the constraints when the tray angle was set to $15^\circ$. The vertical dashed line in the last three images depict the constraints at their limits.