November 18, 2022
What about non-Eucldiean configuration spaces?
(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
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
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
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
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)
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}\]
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
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\))
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}\]
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
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