Further Experimental Results

Dynamic Obstacle Avoidance

Effect of Number of Particles

We study the effect of number of trajectories sampled per iteration of optimization (or particles) on the controller performance in simulation. For this experiment, 10 end-effector pose targets were used that require significant changes in orientation. Each episode is 700 timesteps long after which the manipulator is reset to the base orientation. The horizon is kept constant at 30 timesteps and all cost function weights are also fixed.

Effect of Cost Terms

Next, we study the effect of different cost terms. We first test the constraint based terms i.e. self collision and joint limit avoidance followed by the behavior based manipulability and stop costs.

Manipulability Cost

The manipulability cost acts as a regularizer to keep the manipulator away from singular configurations. The box plot below shows that as the weight on the manipulability cost is increased, the pose reaching accuracy improves. However, after a certain threshold, the manipulability cost interferes with pose reaching and the position accuracy decreases. Maintaining high manipulability allows the robot to reach different end effector orientations accurately.

Stop Cost

The stop cost penalizes joint velocities that are too high for the robot to safely stop within horizon based on a maximum acceleration threshold. An important consequence of this term is that it allows the robot to smoothly stop at the goal even with a short horizon. We demonstrate this effect in the plot below where a lower weight on the stop cost leads to undesirable oscillations near the goal.

Effect of Horizon Length

Here we show a fundamental limitation of the MPC paradigm due to finite lookahead horizon. If the horizon is too short as in the video on the left (H=15), the robot can get stuck in local minima induced by the large wall obstacle and is unable to reach the goal. In practice, the horizon needs to be carefully tuned in order to avoid such corner cases which can be cumbersome. A possible method to overcome this is to use a learned Q function as the terminal cost which increases the effective horizon of MPC.

Effect of Sampling Strategy

Pseudo-random vs Halton Sampling

Halton Sampling is a Quasi Monte-Carlo method that provides better coverage of the action space as compared to pseudo-random sampling that exhibits and undesired clustering of samples. We study the improvement gain by using Halton sampling in low particle regime. The plots below show the quaternion errors achieved by pseudo random and Halton samples with 100 and 500 particles respectively. With lesser number of particles, pseudo-random sampling with comb filter is unable to achieve a quaternion error with a confidence interval within 5% whereas Halton sampling achieves less than 5% quaternion error with both 100 and 500 particles.

Comb Filtering v/s B-Splines

Comb filtering smoothens out the sampled trajectories using user defined filtering coefficients. In order to ensure smooth (low jerk) motions a very strong filtering is required which prevents the robot from ramping up this velocity. Fitting B-Splines to sampled actions is able to ensure smooth acceleration profiles which allows the robot to smoothly ramp up the velocity but comes at the price of reduced pose accuracy. However, since our sampling strategy allows to arbitrarily mix different kinds of trajectories, we create a hybrid sampling strategy (denoted as "mixed") with a ratio of 0.6 and 0.4 of B-Spline and comb filtering. This provides comparable accuracy to comb filtering while also achieving high joint velocities.

Timing Benchmark

Learned v/s Baseline Self Collison Detection

Below we present a timing benchmark for an increasing batch size of query configurations. The learned function is over 40x faster on average than baseline self collision detection that uses forward kinematics to compute link poses and calculates minimum distance between them. Further, the learned self-collision detector maintains a very low latency of 0.4-0.6ms even for large batch sizes.

Gain from Tensorized Forward Model on GPU

The timing benchmark below shows the computational gains from our tensorized GPU based implementation of forward model versus a CPU baseline for varying horizon and number of particles used in MPC.

Comparison to Riemannian Motion Policies (RMPs)

In order to show the gain from forward lookahead in MPC, we compare our framework with RMPs [Ratliff et. al., 2018] in simulation. In the examples shown below, a Franka robot arm is trying to reach different goal points inside the cabinet. This example demonstrates how RMPs can get stuck in local minima when the goal location is on the other side of the cabinet wall. On the contrary, MPC, owing to its forward lookahead, is able to escape such local minima and successfully reach the goal.