November 18, 2022
Shortest Paths in Graphs of Convex Sets, Marcucci et. al.
Motion Planning around Convex Obstacles with Convex Optimization, Marcucci et. al.
What about non-Eucldiean configuration spaces?
Robust Combined Adaptive and Variable Structure Adaptive Control of Robot Manipulators, Yu
(No joint limits)
A manifold is a locally-Euclidean topological space
Continuous functions take nearby points to nearby points
A rule for which functions are continuous by defining what "nearby" means
Explicitly: a rule for constructing arbitrarily small* neighborhoods around points
Examples:
A topological space is path connected if there is a continuous curve joining any two points.
A continuous curve is the image of a function \(f:[0,1]\to X\)
A set is compact if every open cover has a finite subcover
Heine-Borel Theorem: In Euclidean spaces, compact iff closed and bounded
Extreme Value Theorem: Functions over compact sets have max and min values (useful for optimization theory)
A manifold is a locally-Euclidean topological space
A manifold is a locally-Euclidean topological space
Introduction to Smooth Manifolds, Lee
Locally-Euclidean: any \(x\in\mathcal{M}\) is contained in an open set \(U\subseteq\mathcal{M}\) such that \(U\) is homeomorphic to a subset of Euclidean space
Introduction to Smooth Manifolds, Lee
A manifold is a locally-Euclidean topological space
Coordinate chart: An open set (sufficiently small neighborhood) \(U\subseteq\mathcal{M}\) with a homeomorphism \(\psi\)
Atlas: A collection of charts that cover \(\mathcal{M}\)
Transition Map: How to move from one chart to another
Introduction to Smooth Manifolds, Lee
A manifold is smooth if its atlas has smooth transition maps
Intuition: no corners or edges
Can have different levels of smoothness (\(n\)-times continuously-differentiable vs analytic)
We can compute directional derivatives for functions
We can take integrals of functions, vector fields, etc.
Interesting, but not relevant for the current project
Given a point \(x\in M\), we can consider the velocity of curves that pass through \(x\)
This forms a vector space \(T_xM\), called the tangent space
Let \(M\) and \(N\) be two manifolds, and \(\phi:M\to N\) a smooth function
We can use \(\phi\) to "move information" between \(M\) and \(N\)
The pushforward \(d\phi\) moves tangent vectors from \(M\) to \(N\)
The pullback \(\phi^*\) moves functions from \(N\) to \(M\)
\(\phi^*f=f\circ\phi\)
Riemannian Motion Policies use pushforwards and pullbacks to unify controllers across different configuration spaces
RMPs for Safe Impedance Control in Contact-Rich Maniuplation, Shaw et. al.
The Cartesian product of two manifolds is itself a manifold
What if a manifold is also a group?
Need the group operation and inversion to be smooth maps
Examples: SO(3), SE(3)
Coordiante Independent Convolutional Networks -- Isometry and Gauge Equivariant Convolutions on Riemannian Manifolds, Weiler et. al.
Arc length depends on the parameterization of the charts
Triangles and angles look different
A Riemannian Metric assigns an inner product to each tangent space
The inner product must be a positive definite bilinear form, and it must vary smoothly along the manifold
Allows us to define curvature, and compute distances
Consider a piecewise-differentiable curve \(\gamma\in\mathcal{C}^{1}_{\operatorname{pw}}([a,b],\mathcal{M})\)
Its arc length is \(\displaystyle L(\gamma)=\int_a^b \sqrt{\left<\gamma'(s),\gamma'(s)\right>^{\mathcal{M}}_{\gamma(s)}}\,ds\)
For any two points \(p,q\in\mathcal{M}\), the distance between them is defined to be
\[\inf\{L(\gamma)\,|\,\gamma\in\mathcal{C}^{1}_{\operatorname{pw}}([0,1],\mathcal{M}),\gamma(0)=p,\gamma(1)=q\}\]
This integral is parameterization-independent
A geodesic is a (locally) length-minimizing curve -- solution to
\[\operatorname{arg}\,\inf\{L(\gamma)\,|\,\gamma\in\mathcal{C}^{1}_{\operatorname{pw}}([0,1],\mathcal{M}),\gamma(0)=p,\gamma(1)=q\}\]
Variational calculus problem -- solution satisfies Euler-Lagrange system of differential equations
As a result, each geodesic is uniquely defined by its initial conditions
Computation: forward integrate the differential equations
Recall: tangent vectors are velocities of curves through a point
Exponential map intuition:
Configuration space \(\mathcal{Q}\) as a Riemannian manifold
\(\mathcal{M}\subseteq\mathcal{Q}\) is the set of collision free configurations, \(\bar{\mathcal{M}}\) its closure
The shortest path between \(p,q\in\bar{\mathcal{M}}\) is the solution to the optimization problem
\[\begin{array}{rl} \argmin & L(\gamma)\\ \textrm{subject to} & \gamma\in\mathcal{C}^{1}_{\operatorname{pw}}([0,1],\bar{\mathcal{M}})\\ & \gamma(0)=p\\ & \gamma(1)=q\end{array}\]
Re-visiting Riemannian geometry of symmetric positive definite matrices for the analysis of functional connectivity, You et. al.
A set \(X\subseteq\mathbb{R}^n\) is convex if \(\forall p,q\in X\), the line connecting \(p\) and \(q\) is contained in \(X\)
A set \(X\subseteq\mathcal{M}\) is strongly geodesically convex if \(\forall p,q\in X\), there is a unique length-minimizing geodesic (w.r.t. \(\mathcal{M}\)) connecting \(p\) and \(q\), which is completely contained in \(X\).
Sets can be convex in local coordinates, but not geodesically-convex on the manifold
Geodesics can go "the wrong way around"
Radius of convexity: how large can a set be and still be convex?
A function \(f:X\to\mathbb{R}\) is convex if for any \(x,y\in X\) and \(\lambda\in[0,1]\), we have
\(f(\lambda x+(1-\lambda)y)\le\)
\(\lambda f(x)+(1-\lambda) f(y)\)
A function \(f:\mathcal{M}\to\mathbb{R}\) is geodesically convex if for any \(x,y\in \mathcal{M}\) and any geodesic \(\gamma:[0,1]\to\mathcal{M}\) such that \(\gamma(0)=x\) and \(\gamma(1)=y\), \(f\circ\gamma\) is convex
A directed graph \(G=(V,E)\)
Mixed-Integer Riemannian Optimization? No existing literature.
Better option: transform into a GCS problem
For each \(v\in V\), let \(\psi_v:\mathcal{Y}_v\to\mathbb{R}^n\) be a coordinate chart
Define \(X_v=\psi_v(\mathcal{Y}_v)\)
For each \(e=(u,v)\in E\), new edge cost:
\[\ell_e(x_u,x_v)=\ell_e^\mathcal{Y}(\psi_u^{-1}(x_u),\psi_v^{-1}(x_v))\]
If all \(X_v\) and \(\ell_e\) are convex, we have a valid GCS problem
Mixed-Integer Riemannian Optimization? No existing literature.
Better option: transform into a GCS problem
Configuration space \(\mathcal{Q}\) as a Riemannian manifold
\(\mathcal{M}\subseteq\mathcal{Q}\) is the set of collision free configurations, \(\bar{\mathcal{M}}\) its closure
The shortest path between \(p,q\in\bar{\mathcal{M}}\) is the solution to the optimization problem
\[\begin{array}{rl} \argmin & L(\gamma)\\ \textrm{subject to} & \gamma\in\mathcal{C}^{1}_{\operatorname{pw}}([0,1],\bar{\mathcal{M}})\\ & \gamma(0)=p\\ & \gamma(1)=q\end{array}\]
\(\mathcal{Q}\) is zero-curvature
Fetch Robot Configuration
Space: \(\operatorname{SE}(2)\times \mathbb{T}^3\times \mathbb{R}^7\)
\(\mathcal{Q}\) is zero-curvature
\(\mathcal{A}=\{(X_v,\psi_v)\}_{v\in V}\) is a finite atlas of \(\mathcal{M}\), with each \(X_v\) strongly geodesically convex, and each \(\psi_v\) an isometry
How to move from one chart to another
Introduction to Smooth Manifolds, Lee
Decision variables:
\(\forall v\in V\), we have \(x_v=(x_{v,0},x_{v,1})\in\psi_v(X_v)^2\)
Edge costs:
\[\ell_e(x_u,x_v)=||x_{u,0}-x_{u,1}||_2,\quad\forall e=(u,v)\in E\]
Edge constraints:
\[\tau_{u,v}(x_{u,1})=x_{v,0},\quad\forall e=(u,v)\in E\]
(\(\tau_{u,v}\) is the transition map from \(X_u\) to \(X_v\))
Motion Planning around Obstacles with Convex Optimization, Marcucci et. al.
Want all equations to live in Euclidean coordinates
Equivalent conditions:
\[\psi_u^{-1}(x_{u,1})=\psi_v^{-1}(x_{v,0})\;\;\Leftrightarrow\;\;\tau_{u,v}(x_{u,1})=x_{v,0}\]
Motion Planning around Obstacles with Convex Optimization, Marcucci et. al.
Square with opposite edges identified
Configuration Space: \(\mathbb{T}^2\)
A robot arm with 5 continuous revolute joints in the plane
Configuration Space: \(\mathbb{T}^5\)
(No self collisions)
Modified to be a continuous revolute joint
Key idea: generate within a chart in the first place
Use IRIS in configuration space, add bounds on the diameter of the set to preserve strong convexity
A Panoramic View of Riemannian Geometry, Berger
All Bézier curve constructions also hold on the manifold
For higher-order continuity constraints, want tangent vectors to be equal. Use the pushforward!
Order \(0\) continuity, order \(k\) splines:
\[\tau_{u,v}(x_{u,k})=x_{v,0}\quad\forall e=(u,v)\in E\]
Order \(m\) continuity, order \(k\) splines:
\[(\tau_{u,v})_*(x^{(m)}_{u,k})=x^{(m)}_{v,0}\quad\forall e=(u,v)\in E\]
If \(\tau_{u,v}\) is an isometry, then \(\tau_{u,v}(x)=R(x)+T(x)\) for \(R\in\operatorname{O}(n)\) and \(T\in\operatorname{E}(n)\). Thus, \((\tau_{u,v})_*=R\).
In some (most?) cases, we can align the coordinate systems
If so, we have \(\tau_{u,v}\in \operatorname{T}(n)\) and \((\tau_{u,v})_*=I_n\), \(\forall e=(u,v)\in E\)
November 18, 2022