iRS-RRT
Pang, Terry, Lu
the Story
Q: how do we solve long horizon, planning through contact problems?
- Deep learning is impressive. But learned policies do not generalize to different tasks.
- So far, methods based on physics models suffer from the huge number of contact modes.
- Contact modes manifest as complementarity constraints in trajectory optimization. They can be mitigated with numerical tricks, but in general are sensitive to initial guesses and cost tuning.
- For simpler systems, contact modes can be enumerated. Discrete mode planning and continuous state planning can be interleaved in a sampling-based planning algorithm. But this would not scale to tasks such as 3D dexterous manipulation.
- In this work, we propose to replace the reasoning about contact modes with their statistical summary into locally linear systems, which we call bundled dynamics.
- We show the power of bundled dynamics with some simple modifications to the standard kino-dynamic RRT.
Reachable Set on Bundled Dynamics
Let's motivate the Gaussian from a different angle.
Recall the linearization of bundled dynamics around a nominal point.
Fixing the state, we reason about the states that are reachable under the bundled dynamics under some input.
(Image of the input norm-ball under bundle dynamics, assume \bar{u}_t is zero.)
The latter expression becomes the eps-norm ball under Mahalanabis metric.
Distance Metric Based on Local Actuation Matrix
Consider the distance metric:
This naturally gives us distance from one point in state-space another as informed by the actuation
(i.e. 1-step controllability) matrix.
NOTE: This is NOT a symmetric metric for nonlinear dynamical systems. (even better!)
Rows of the B matrix.
A is harder to reach (has higher distance) than B even if they are the same in Euclidean space.
Handling Singular Cases
What is B is singular, such that
The point that lies along the null-space is not reachable under the current linearization. So distance is infinity!
Numerically, we can choose to "cap" infinity at some finite value by introducing regularization.
Rows of the B matrix.
There is loss of 1-step controllability and A is unreachable. Infinite distance.
Connection to Gaussian Covariance Estimation
We can obtain the covariance matrix of the ellipse directly using gradient information, but we can also consider the zero-order version that solves the least-squares problem:
Under the least-squares solution, one can show a connection between directly doing covariance estimation on samples.
Consider putting the above into data matrix form
where X is data matrix of u that is sampled from zero-mean Gaussian and diag. covariance of sigma.
How bundled dynamics helps in kino-dynamic RRT
- RG-RRT uses local reachability information to guide subgoal sampling towards more reachable part of the smooth space.
- Bundled dynamics provides good reachability info for systems with contact.
- By helping with 1 or T-step trajectory optimization in extension (the same story as iRS-MPC).
- There's a trade-off between the number of nodes and the planning horizon T.
Rejection Sampling / Contact Sampling
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1499994/images/9354871/Figure_1.png)
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1499994/images/9354873/Figure_no_regrasp.png)
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1499994/images/9354874/Figure_global.png)
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1499994/images/9354875/Figure_no_regrasp_detailed.png)
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1499994/images/9354876/global.png)
Rejection Sampling + Contact Sampling
No Contact Sampling
No Rejection Sampling
Results
- Planar pushing
- Planar hand
- Allegro hand
Planar Hand
1-step extension
20-step extension
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/9354865/pasted-from-clipboard.png)
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/9354869/pasted-from-clipboard.png)
(b1)
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/9366721/newplot__5_.png)
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/9366723/newplot__4_.png)
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/9366730/newplot__8_.png)
(b2)
(b1)
(b2)
(c1)
(c2)
(c1)
(c2)
(a)
(b)
(c)
(d)
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/9366937/contact_volume.png)
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/9366937/contact_volume.png)
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/9366721/newplot__5_.png)
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/9366723/newplot__4_.png)
(a)
(b)
(c)
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/9367783/newplot__2_.png)
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/9366721/newplot__5_.png)
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/9366723/newplot__4_.png)
(a)
(b)
(c)
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/9367822/newplot__5_.png)
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/9366937/contact_volume.png)
(d)
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/9366721/newplot__5_.png)
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/9366723/newplot__4_.png)
(a)
(b)
(c)
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/9367822/newplot__5_.png)
(d)
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/9567809/pasted-from-clipboard.png)
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/9567791/pasted-from-clipboard.png)
![](https://s3.amazonaws.com/media-p.slid.es/uploads/1354033/images/9567850/pasted-from-clipboard.png)
planning_through_contact_Feb_2022
By Pang
planning_through_contact_Feb_2022
- 32