Torque-Limited Manipulation Planning with Contact
Ramkumar Natarajan, Maxim Likhachev and Howie Choset

Abstract—By making contact with the environment and leveraging the contact dynamics, robot manipulators can reachconfigurations well outside their static reachability and carry
payload outside their capability (Fig 1). We adapt our previously
developed INSAT algorithm [1] to tackle the problem of torquelimited manipulation planning through contact. INSAT requires
NO prior over contact mode sequence and NO initial templateor seed for trajectory optimization. INSAT achieves this byinterleaving graph search to explore the joint configuration space(which determines the contact mode) with several incremental
trajectory optimizations seeded by neighborhood solutions to finddynamically feasible trajectories. We demonstrate our resultswith the pick and place of an overweight payload by leveraging
robot-environment contact on a Kinova Gen3 robot in MuJoCo.

I. Approach Overview
Our method consists of two interleaved components runningconcurrently, a low-dimensional discrete graph-search in the
manipulator configuration space and the high-dimensional
trajectory optimization in the joint velocity and contact model
parameter space. We first explain the trajectory optimization
setup used to generate the full dimensional edge in the graphwhose cost is used to drive the low-dimensional search. We
then describe the adaptation of INSAT for our application.
A. Trajectory Optimization for Planning through Contact
The trajectory optimizer is set up to solve a boundary
value problem by finding a joint torque input trajectory thatconnects two manipulator configurations. We solve the trajectory optimization problem using Successive Convexification(SCvx) [2]. SCvx solves a sequence of smooth quadraticapproximations of the original nonlinear problem subjected
to linearized dynamics. But, as the hybrid system dynamics
with contact is discontinuous, the linearization of dynamics ispoor. To alleviate this, we use the tunable soft contact modelin MuJoCo [3] to iteratively solve the trajectory optimization
problem (Eq. 1). We begin with the relaxed setting for thecontact model (non-zero virtual contact forces even when notin contact) in which the hybrid system dyanamics (manipulator
dynamics + contact dynamics with the environment) is smoothand solve the Eq. 1 using SCvx. We then iteratively tighten
the contact model softness and re-solve Eq. 1 by warm-startingwith the solution from the previous relaxed iteration.

min𝝉 [.]∥x[𝑁] − x𝑔𝑜𝑎𝑙 ∥ 2 +𝑁−1∑︁∥𝝉[𝑖] ∥ 2(1a)Fig. 1: 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.
where the state x contains joint angles and joint velocity, 𝝉 isthe joint torque vector and f is the hybrid system dynamics.
B. INSAT: INterleaved Search And Trajectory Optimization

Fig. 2: A schematic of working principle of INSAT
We refer the reader to [1] for the detailed working principle of INSAT. Fig 2 and 3 provide visual support for theapplication of INSAT algorithm to torque-limited manipulation planning through contact and its working on a planar
example. INSAT performs interleaved search on discrete lowdimensional manipulator configuration space and continuous
high-dimensional joint velocity and contact model parameter
space. The low dimensional search gets the manipulator aroundobstacles and evaluates various contact mode sequences. The
high-dimensional trajectory optimization validates or invalidates the dynamic feasibility of paths discovered by the lowdimensional search. Consequently, INSAT generates dynamically feasible trajectory for the manipulator to brace with theenvironment, offset/stay within its torque limits, and reach thedesired goal.

𝑖=0s.t.

x[𝑖 + 1] = f(x𝑖 , u𝑖 ),(1b)

II. Simulation Results(1c)

Our algorithm is tested in simulation on Kinova Gen3
manipulator. The planner discovers the right contact behavior
to carry the payload and swings to pump energy into thesystem and drive to the goal (see Fig. 4 caption).

x(0) = x0 , x(𝑁) ∈ X𝑔𝑜𝑎𝑙| x¤ (𝑡)| ≤ x¤ lim(1d)|𝝉(𝑡)| ≤ 𝝉 lim , 𝝉 ∈ U(1e)Fig. 3: Graphical illustration of INSAT using a 2D planar arm lifting a payload by making contact. The low-dimensional graph is shownwith grey nodes and edges (thicker nodes correspond to expanded ones) and the high-dimensional trajectory optimization output is shownwith splines connecting the nodes. For the sake of visual clarity, the nodes here denote the end effector configuration of the arm. In thelow-dimensional graph, the start node is shown in red, goal in green, newly generated node from vertex expansion in blue, waypoint nodefor INSAT repair phase [1] in pink. In the high-dimensional graph, the trajectory optimization output to previously expanded nodes andcurrently generated node are shown with blue and red spline respectively. As the objective is to minimize total joint torque, we can see that
INSAT finds a trajectory for the planar arm to swing over the obstacles to grab the payload and lift it by making contact.

Fig. 4: (Row-major order from top-left) Snapshots of trajector generated by INSAT for a pick and place task of an object(shown as red sphere) using Kinova Gen3 robot from a confined shelf to a table. The mass of the object is set to violate thestatic torque limits of the manipulator (when no contact support from the environment) at the start configuration. The robot
maintains the contact with the shelf as much as possible by dragging the object out and swinging across its base to pumpenergy to eventually carry the object on to the table. The task requires reasonably long horizon planning in which standalone
trajectory optimization struggles. By guiding the trajectory optimization with graph search over manipulator configurations,
INSAT is able to produce a dynamically feasible trajectory of interesting behavior.

References[1] Ramkumar Natarajan, Howie Choset, and Maxim
Likhachev. Interleaving graph search and trajectory optimization for aggressive quadrotor flight. IEEE Roboticsand Automation Letters, 6(3):5357–5364, 2021.
[2] Aykut Özgun Önol, Philip Long, and Taşkın Padır.
Contact-implicit trajectory optimization based on a vari-able smooth contact model and successive convexification.
In 2019 International Conference on Robotics and Automation (ICRA), pages 2447–2453. IEEE, 2019.
[3] Emanuel Todorov, Tom Erez, and Yuval Tassa. Mujoco:
A physics engine for model-based control. In 2012
IEEE/RSJ International Conference on Intelligent Robotsand Systems, pages 5026–5033. IEEE, 2012.