Long Horizon Planning through Contact using Discrete Search and Continuous Optimization

TL;DR

This paper proposes Lazy INSAT with reduced optimization rejection that systematically procrastinates its calls to trajectory optimization while reusing feasible solutions that violate boundary constraints and shows that Lazy INSAT can discover solutions for tasks that cannot be accomplished within its design limits and without interacting with the environment.

Abstract

Robots often have to perform manipulation tasks in close proximity to people. As such, it is desirable to use a robot arm that has limited joint torques to not injure the nearby person and interacts with the environment to explore new possibilities for completing a task. By bracing against the environment, robots can expand their reachable workspace, which would otherwise be inaccessible due to exceeding actuator torque limits, and accomplish tasks beyond their design specifications. However, motion planning for complex contact-rich tasks requires reasoning through the permutations of different possible contact modes and bracing locations, which grow exponentially with the number of contact points and links in the robot. To address this combinatorial problem, we developed INSAT, which interleaves graph search to explore the manipulator joint configuration and the contact mode space with incremental trajectory optimizations seeded by neighborhood solutions to find a dynamically feasible trajectory through contact. In this paper, we present recent additions to the INSAT algorithm that improve its runtime performance. In particular, we propose Lazy INSAT with reduced optimization rejection that systematically procrastinates its calls to trajectory optimization while reusing feasible solutions that violate boundary constraints. The algorithm is evaluated on a heavy payload transportation task in simulation and on physical hardware. In simulation, we show that Lazy INSAT can discover solutions for tasks that cannot be accomplished within its design limits and without interacting with the environment. In comparison to executing the same trajectory without environment support, we demonstrate that the utilization of bracing contacts reduces the overall torque required to execute the trajectory.

Figures (17)

• Figure 1: Kinova Gen3 manipulator transferring a long heavy wooden plank across a partitioned space by bracing against the environment for support.
• Figure 2: An example of a hyperredundant robot manipulator lifting a heavy tool in a confined space by leveraging contact with the environment to assist a human worker.
• Figure 3: One iteration of node expansion, successor generation and full-D trajectory optimization in INSAT. Here $\textbf{x}^S$ and $\textbf{x}^G$ are full-D start and goal states. The low-D graph is denoted using black dots and arrows as nodes and edges. The one-to-many mapping of low-D node to full-D space is shown using the gray circle around the low-D node. The red curves denote the full-D dynamically feasible trajectory. ROW MAJOR ORDER: (1) An intermediate step in INSAT algorithm (2) INSAT selects the node to be expanded (encircled in green) (3) and generates the low-D successors (orange nodes). (4) INSAT searches over all the ancestors of the newly generated successor (encircled with a dotted purple line) to find a valid full-D trajectory to the successor. (5)-(6) Invalid trajectory from some of the ancestors. (7)-(11) Once a valid trajectory is found, the trajectory optimization is warm-started with the incoming trajectory of the valid ancestor and the incremental trajectory found in the previous step to find the valid full-D dynamically feasible trajectory from start $\textbf{x}^S$. (12) The same process is repeated for the other newly generated successor.
• Figure 4: Working principle of INSAT
• Figure 5: The tunable smooth (except at $\dot{\psi}_n(\textbf{q}, \dot{\textbf{q}})=0$) contact friction model that supplies virtual frictional force to break static friction in trajectory optimization.