Automated Derivation of Behavior Vocabularies for Autonomous Humanoid Motion. Jenkins, Mataric. AAMAS 2003

1. Automatic derivation of motion primitives and means of combining them from human motion data using isomap
2. For robot control, useful to have a restricted set of primitives to search over to do motion instead of planning in full space which is huge.  Sometimes this set can be defined by hand, but this has obvious limitations (time-intensive, requires domain expertise, even then may be incorrect)
3. Paper extends isomap to extract spatio-temporal structure
   1. Finding motion primitives relies on additional clustering and interpolation
4. Here they use the method for motion synthesis
5. 
6. “In Motion Textures, <another approach> motion segments are learned so they can be accurately reproduced by a linear dynamical system within some error threshold.”
   1. Here the goal is “… to produce primitive behaviors that have an observable theme or meaning.”
   2. Method here requires some domain knowledge
7. A number of references, but this work and the motion texture seem the most relevant
8. “Each  extracted feature represents a group of motions with the same underlying theme and can then be used to construct a primitive motion module realizing the theme.”
9. Using straight PCA has been used, but the features that come out from that aren’t really useful
   1. Can also do PCA and then clustering on top of that, but unless the # of clusters is known a-priori, the results also come out pretty meaningless.  Also the clustering just gives meaningful results on data that falls within the corpus used.  It doesn’t extrapolate well
10. Using vanilla isomap, locally linear embedding, and kernel pca will all have same qualitative drawbacks as vanilla pca
11. Here they do Isomap, but add temporal component
12. 
13. “The Isomap embedding unravels the spatial structure of the S-cure removing the ‘s’ nonlinearity, producing the flattened data indicative of the 2-manifold structure of the S-curve.  However, the model that has generated the data are a 1-manifold with an S-curve nonlinearity and multiple translated instances.  Spatio-temporal Isomap produces an embedding indicative of this 1-manifold structure.  This embedding both unravels the S-curve nonlinearity and collapses corresponding points from multiple instances of the S-curve to a single point.”
14. The idea behind isomap (as well as kernel PCA) is to do eigen decomposition on a similarity matrix in feature space as opposed to a covariance matrix (which is used in PCA)
    1. “The feature space is a higher dimensional space in which a linear operation can be performed that corresponds to a nonlinear operation in the input space.  The caveat is that we cannot transform the data directly to feature space.  For performing PCA in a feature space, however, we only require the dot-product (or similarity) between every pair of data points in feature space.  By replacing the covariance matrix C with the similarity matrix D, we fit an ellipsoid to our data in feature space that produce nonlinear PCs in the input space.”
15. Spatio-temporal isomap works the same way, but an extra step is added to deal with temporal dependencies.  As compared to vanilla isomap “If the data have temporal dependency (i.e., a sequential ordering), spatial similarity alone will not accurately reflect the actual structure of the data.”
16. They tried two method of dealing with temporal dependencies
17. Spatial neighbors are replaced by adjacent temporal neighbors, which introduces a first-order Markov dependency into the embedding
18. Creates connected temporal neighbors (CTNs), as well as CTN connected components
    1. “Points not in this CTN connected component will be relatively distal.  Thus, CTN connected components will be separable in the embedding through simple clustering.”
19. Processing kinematic motion:
    1. 1st iteration makes clusterable groups of motion, primitive feature groups
    2. Interpoliation is done on primitive feature groups to make parameterized primitive motion modules
    3. Spatio-temporal isomap is again applied to first embedding to make more clusterable groups of motion called behavior feature groups
    4. From a behavior feature group, meta-level behavior encapsulates component primitives and links them with transition probabilities
20. Feels like its doing things pretty similar to slow feature analysis, “Incremental Slow Feature Analysis:
    Adaptive Low-Complexity Slow Feature Updating from High-Dimensional Input Streams” does mention this work as similar but a little different.
21. First step requires motion segmenting.  They use both ground truth as well as an algorithm called kinematic centroid segmentation 
    1. SFA doesn’t require this, the segmenting falls out by itself
    2. <I think this is a bit of a deal breaker for us>
22. Seems like they use a pretty simple clustering method <they call it “sweep and prune”, never heard of it>
23. Set of motion segments in each feature group are treated as exemplars.  Variations on that can be constructed by interpolating (many different ways of doing the interpolation) between exemplars
24. Processing sequence does:
    1. primitive features -> behavior features -> meta-level behaviors
25. <skimming the rest because the segmentation requirement for initial processing probably makes this unusable for us>

In Transit to Constant Time Shortest-Path Queries in Road Networks. Bast, Funke, Matijevic, Sanders, Schultes. Algorithm Engineering and Experiments (ALENEX)

1. Beats previous best results by two orders of magnitude <was also published in Science, but the article is so short I’m not taking notes on it>
2. In worst-case Dijkstras cannot be below linear because it may have to examine all nodes and vertices
   1. The only way to beat this is by preprocessing
3. Constant query time can be achieved by superlinear storage <Upon looking up that reference, it really looks linear.  The response may be slightly suboptimal (but this amount is constant in the size of the graph)>
4. The improvements in quality and speed here exploit the fact that roads have particular properties, like low branching factors, fact that highways are designed to be used for long distance travel…
5. Idea of transit node is “that for every pair of nodes that are ‘not too close’ to each other, the shortest path between them passes through at least one of these transit nodes.”
   1. Additionally, even when travelling very far, the number of transit nodes that must be used is very small (like 10)
6. In the US road data they consider, 24 million nodes, and 10,000 transit nodes
7. Example (paper has figure): “Finding the optimal travel time between two points (flags) somewhere between Saarbr¨ucken and Karlsruhe amounts to retrieving the 2 × 4 access nodes (diamonds), performing 16 table lookups between all pairs of access nodes, and checking that the two disks defining the locality filter do not overlap. Transit nodes that are not relevant for the depicted query are drawn as small squares.”
8. Mentions that there are special classes of algorithms for working on planar graphs, which roads (basically) are
9. They have a previous paper that hierarchically builds paths based on increasingly long paths <should check out http://algo2.iti.kit.edu/schultes/hwy/esa06HwyHierarchies.pdf>
   1. This paper is by the same authors, and ends up doing something very similar to transit nodes (they are just nodes high in the hierarchy).  In the earlier paper access nodes were computed on the fly whereas here they are precomputed
   2. It also doesn’t deal with how to figure out the distance in the distance table is indeed the shortest distance
10. A* is actually one of the best methods for real world planning (when done around landmarks and the the triangle inequality).  Here, access nodes can be used as the landmarks
11. The hierarchy approach also needs to be grounded in actual geographic information to work <at small scales?>
12. Nodes are labeled either close/not close to individual access nodes
13. Access nodes are found by backwards search by Dijkstra’s, and do this until all paths converge on another transit node, then take all nodes found through this search that do not reach another transit node
14. There are a number of ways to implement a locality filter; the one they recommend is to examine everything that is reachable within a cluster in the hierarchy
15. This information allows for what is effectively constant time queries for shortest paths
16. There is another way to implement the algorithm that is grid based – uses 2 grids of different resolution
    1. Needs some optimization to function efficiently
    2. Computing access nodes in grid-based implementation is very simple
17. With the optimizations, long-distance queries via transit nodes is actually faster (1 million x) than local queries.  Although on average local queries that dont go through access nodes are about 1% of queries <not sure how this is measured>  it still dominates the running time
    1. There is a scheme also that allows speedups for local queries as well
18. In the grid setting, there are tradeoffs between space, number of transit nodes <both increasing with resolution> and #local queries that don’t go through transit nodes <decreasing with resolution>
19. Ah, to get the best of both worlds they use a hierarchy of grids
20. The exact algorithm used to compute access nodes and transit nodes is confusing, and their diagram doesn’t help either.  Its written in English but would be much more understandable written in math (or at least pseudocode)
21. Queries are super-easy to run (if nodes are more than 4 cells apart, path must go through a transit node, which already have pairwise distances computed)
22. Can do hierarchical <seems to be just multiple griddings of different resolutions but I think there is another paper that may go into it more thoroughly>
23. Do a greedy set cover to optimize storage for access nodes
24. Implemented on data with 24m nodes, 60m edges
25. Run on a ’07 dual-proc machine, 99% of queries have an avg time of 12 microseconds (adding the extra 1% moves it to 63 msec – are those the ones that don’t go through transit nodes?)
26. Computation times are not “Dijkstra rank dependent”
27. <Ok that’s not the end of the paper – just for the grid part, now moving on to a more fully hierarchical method>
28. Highway hierarchies
29. An edge is a highway edge is an edge that is in the shortest path between two vertices, but not in the local neighborhood of those vertices
30. They also may contract edges based on a bypassbility criterion, but that isn’t yet discussed at length
31. The hierarchy is then constructed by recursively building highway networks on top of each preceeding layer
32. With increasing path distance, paths will go increasingly higher in the highway hierarchy
33. So the nodes existing at some level in the hierarchy can be used as the transit nodes
34. #transit nodes controlled by neighborhood size and level of hierarchy
35. “Note that there is a difference between the level of the highway hierarchy and the layer of transit node search” <on this read through the distinction is actually not clear to me – looks like they are basically equivalent but in reverse order?>
36. There is a gloss over how description of how access nodes are found – another paper is referenced <which I will try and get to> and point #20 is referenced
37. Present 2 methods of the algorithm, which have different tradeoffs in terms of space, preprocessing, and query time
38. Search is done top-down
39. “For a given node pair (s, t), in order to get a complete description of the shortest s-t-path, we first perform a transit node query and determine the layer i that is used to obtain the shortest path distance. Then, we have to determine the path from s to the forward access node u to layer i, the path from the backward access node v to t, and the path from u to v”
40. There is still room for improvement for the locality filters
41. For US data, 99% of cases are handled at the top layer
42. Search times start low and increase as searches are still local but further away, and then drop extremely low once nodes are far enough that transit nodes can be leveraged
43. <On to conclusion>
44. “Building on highway hierarchies, this can be achieved using a moderate amount of additional storage and precomputation but with an extremely low query time. The geometric grid approach on the other hand allows for very low space consumption at the cost of  slightly higher preprocessing and query times.”
45. “There are many interesting ways to choose transit nodes. For example nodes with high node reach [9, 6] could be a good starting point. Here, we can directly influence |T |, and the resulting reach bound might help defining a simple locality filter. However, it seems thatgeometric reach or travel time reach do not reflect the inhomogeneous density of real world road networks. Hence, it would be interesting if we could efficiently approximate reach based on the Dijkstra rank.
    Another interesting approach might be to start with some locality filter that guarantees uniformly small local searches and to view it as an optimisation problem to choose a small set of transit nodes that cover all the local search spaces.”

STOMP: Stochastic Trajectory Optimization for Motion Planning. Kalakrishnan, Chitta, Theodorou, Pastor, Schaal

1. Another planner that uses optimization to minimize cost
2. Performs local search by starting with an initial candidate trajectory and then introducing noise to sample nearby it:

   1. STOMP is an algorithm that performs local optimization, i.e. it finds a locally optimum[al] trajectory rather than a gobal one.  Hence, performance will vary depending on the initial trajectory used for optimization.  STOMP cannot be expected to solve typical motion planning benchmark problems like the alpha puzzle in a reasonable amount of time.

3. Compares itself to the algorithm CHOMP, which is similar but CHOMP is a gradient-based method.  Because STOMP is not a gradient method, and has some randomized exploration, it has a better ability to escape local optima than CHOMP
4. Because it is not a gradient method, STOMP can be used with wider classes of cost functions (such as those hard constraints on angles or torques which many method have trouble with), and more general constraints
   1. Hard limits can also cause local optima, so while the method doesn’t just break in these settings, it is possible convergence will not be to optimal resutls
5. Basic motion planning algorithms generally require smoothing and other further transformations after a solution is found in order to maintain energy efficiency, minimize jerk (and motor wear), among humans abrupt changes caused by classical motion planning methods can be disconcerting.
   1. A result from a classical motion planner could be used as the initial proposal for this algorithm
6. In contrast to the classical motion planning approach (which they call sampling-based), are optimization based planners which minimize cost
7. Seems like this method also uses inverse kinematics, runs on a domain that does is *not* underactuated or exhibit drift: “Specifically, we consider trajectories of a fixed duration T, discretized into N waypoints, equally spaced in time.”
8. Also does an EM-style update, and weighs samples according to their goodness: “The trajectory updates can be considered safer than a standard gradient descent update rule.  The new trajectory is essentially a convex combination of the noisy trajectories which already been evaluated, i.e. there are no unexpected jumps to unexplored parts of the state space due to a noisy gradient evaluation.”
9. The only open parameter to the algorithm is the exploration noise
10. In settings where optimization is done in a multidimensional space, each dimension is dealt with independently
11. They do a  grasping task on a PR2 in simulation and on the real robot
    1. CHOMP fails to produce an effective plan about 30% of the time, while STOMP is always successful, probably due to the bit of randomized exploration STOMP does.
12. They mention a paper that introduces noise into CHOMP to help get around local minima, but that it was difficult to use and implement, with many parameters that needed to be tuned

Progress in Algorithmic Motion Planning and Opportunities at the Intersection with Perceptual Science. Kostas Bekris talk.

1. Interests:
   1. High-quality plans that
   2. Respect of physical constraints of the system
   3. Real time constraints are important
      1. Real-time planning may require short planning horizons which can lead to situations where collision cannot be avoided
   4. Multi-agent planning
   5. Interaction with humans
2. Methods cited for control theory: LQR, partial feedback linearization, HJB and pnotyagin’s principle
3. In late 70s, PSPACE-Hardness was established (Reif ’79, Schwartz & Sharir 82, 84)
4. Cites Canny’s work (I just grabbed his thesis from the library), talked about roadmaps, but the algorithm presented was doubly-exponential, never even implemented.
5. PRMs were introduced by Kavarki, Bekris’ advisor in 1996.  An efficient way of producing roadmaps
6. In the general case, planning without respecting dynamics and then trying to smooth the path so it complies with dynamics is not possible
7. He is interested in finding more computationally near-optimal results as opposed to optimal results at the limit with infinitely dense graphs
   1. Do dense planning and then prune
8. His general goal is PAC-style planning
   1. The results in the field are almost entirely at the limit
9. Under differential constraints, PRMs are difficult because of the BVP problem, RRTs dont have that problem
10. Methods of encouraging efficient exploration (otherwise acrobot for example just hangs down with random actions)
11. Can do replanning in domains that are non-static
12. Multi-agent path planning is poly-time (non-optimal) in discrete domains
13. Future work in area of co-robotics (robot, human interaction), maybe from perspective of game theory, but want pareto optimality, introduces a number of other problems, of course
14. They may get a BAXTER(!)
15. Minimax regret: Savage ’51, Niehas ’48
    1. Good for human interaction, because minimax results agree more with human intuition than game-theoretic results

Sufficient Conditions for the Existence of Resolution Complete Planning Algorithms. Yershov, LaValle. Proceedings Workshop on Algorithmic Foundations of Robotics (WAFR), 2010

1. Kobilarov cited this paper saying the following: “Conditions for resolution completeness of planning with such primitives [parameterizations of open-loop control with actions and corresponding time durations] have been established.”
2. Establishes metric spaces over the control and trajectory spaces
3. Based on these metrics, if the domain is Lipshitz, then the mapping between open loop control and trajectories is continuous
4. Because of this, it is possible to search the space of actions, as opposed to building a reachability graph.
5. This algorithm has guarantees like most motion planning algs, which is that if a solution exists, it will be found eventually
6. Not reading this carefully, because I think I already got what I need out of this paper.  Most of it is proofs, which I dont really care about – just what is proved.
7. Requires continuity, which goes with Lipshitz smoothness
8. Algorithm is very simple
9. The algorithm effectively deals with noise, as there is a term that deals with error that arises from integration errors.

Anytime Computation of Time-Optimal Off-Road Vehicle Maneuvers using the RRT*. Jeon, Karaman, Frazzoli. Decision and Control.

1. An empirical application of RRT*, this paper finds trail-breaking as something that naturally emerges from RRT*
2. I was excited because I thought this application would use stochasticity, but looks like it is a deterministic simulation.
   1. They give the dynamics equations
3. There have been methods that applied numerical optimization from optimal control theory to this domain, but it requires “careful transcription to numerical optimization formulations.  Most algorithms of this class must be started with a feasible initial solution, which is usually hard to generate in the first place.”
   1. Also some suffer from local optimality (may be same as requiring initial solution)
   2. Cites applications of optimal control to the domain, as well as a paper that compares other methods
4. In this application, they plan in the “task space”, which is some lower dimensional representation of the original problem that doesn’t throw out information
5. Plan in state space, and then require a method which will near-optimally nearly-steer between the two states
   1. The one they use is based on shooting, the one they use requires another function that subdivides the input and increments or decrements it as needed
   2. That is a pretty difficult problem in itself
   3. Reminds me of binary action search
6. Uses branch and bound/admissible heuristics
7. Approach requires distance metric, as all RRT methods do

Adaptive Path Planning in a Dynamic Environment using a Receding Horizon Probabilistic Roadmap: Experimental Demonstration

1. If dynamics and constraints are nonlinear, there is no closed-form analytical solution
2. Gradient and potential field methods can be useful in nonlinear domains, with restricted observability, but are strongly influenced by initial search location, as nonlinear constraints can introduce many maxima/minima
   1. Gradient methods may require special differentiation because of non-smooth constraints
3. The main idea of the paper is to use PRMs to build a global connectivity graph, which may need to have edges removed as the agent only senses obstacles nearby, so replanning is still needed as locations that were thought to be free turn out to be closed
   1. Planning is done purely in state space, then motion primitives stitch the states together.
4. They plan in a quadratic cost function
5. The use a helicopter domain that they provide simple dynamics equations for
6. The motion primitives used for planning ensure the helicopter does not enter a bad state
7. They also tested the approach on an actual helicopter

Cross-Entropy Motion Planning. Kobilarov. International Journal of Robotics Research. 2012.

1. Not the same research as presented in https://aresearch.wordpress.com/2012/11/05/cross-entropy-randomized-motion-planning-marin-kobilarov-rss-07/
2. This approach is for the noiseless setting
3. The previous paper deals with using cross-entropy directly, while this paper discusses a combination of cross-entropy and RRT*
4. The additional RRT* component is intended to help deal with the more difficult regions of planning “In particular, more complicated obstacles such as those associated with narrow passages significantly shrink the feasible regiouns in trajectory space and thus, in the abscence of a good prior, can render the method intractable due to the large number of rejected samples.”
5. Considers open-loop planning, although mentions there are a two ways to parameterize trajectories:
   1. As a sequence of actions: “Conditions for resolution completeness of planning with such primitives [encoding of sequence of actions with time duration for each action] have been established (Yershov and LaValle 2011).
   2. As a sequence of states
   3. The paper outlines methods for doing both, as either may be better depending on the domain
6. “In general, the problem cannot be solved in closed form since both the dynamics and constraints can be non-linear.  Gradient-based optimization is not suitable unless a good starting guess is chosen since the constraints impose many local minima.  In addition, constraints corresponding to arbitrary obstacles can be non-smooth and require special differentiation (Clarke et al. 1998) to guarantee convergence.”
7. In sampling based motion planning, a graph is produced that is a finite approximation to the infinite set of feasible trajectories, so the original problem can be solved approximately through the graph
8. Care about optimality at the limit.  PRMs approach optimality, but this occurs at an exponentially slow rate (defined how excatly?) due to the increasing number of edges+vertices “which in higher dimensions becomes computationally intractable.”
   1. So this paper cares about RRTs and not PRMs
9. RRT* has a problem of uniformly distributing samples, and most of these samples don’t help at all.  By using cross-entropy optimization on top, it attempts to allocate samples along the path where the optimal trajectory may lie
   1. CE optimizes the method of sampling vertices
   2. RRT* is further optimized to always attempt to connect newly added nodes to the goal
10. “Even though general theoretical convergence of the CE method has been shown [citations] actual rates of convergence, sample complexity, or precise performance guarantees remain open problems.”
11. The difference between RRT* and cross-ent is that one samples in state space, the other in parameterized trajectories (such as action sequences)
12. CE was originally designed for rare-event estimation. “The rare event of interest in this work is finding a parameter Z with a real-valued cost J(Z) which happens to be very close to the cost of an optimal parameter Z*… the rare-event estimation is equivalent to the global optimization of J(Z)”
13. Uses Gaussain Mixture Models as the basis for CE optimizaiton, because it is easy to do with EM
    1. Although it can only capture as many local regions as components in the mixture, each Gaussian component can be regarded as an approximation of a local second-order model of the objective function centered at each mean (by considering the covariance as the inverse Hessian of the cost – I would like a citation/further discussion of this)
14. Discusses connection to CE and CMA as well as another algorithm called EDA.  The connection to CMA was already discussed before in the CMA-ES paper.  Says the two are virtually identical in the Gaussian case.
15. “In case when very few elite samples are available a classical GMM EM algorithm will fail to estimate correct means and covariances.”  So they add additional noise, as is common in Cross-Ent
16. Gives examples of double integrator(actually in 2D, with obstacles), driving (dubins?),  and helicopter navigation
17. In the helicopter domain, 85% of rollouts with regular CE are thrown out, so RRT* is added to make samples more useful
18. In TCE RRT* (There is also an SCE proposed), examines correlations of states across entire trajectories
19. In the helicopter, shows expected optimal trajectory as a result of 3 mixture components (I’m not sure what dimension, though).  The result is very complex in terms of the resulting path in the x,y space.
20. Discusses other distributions that may make a good tradeoff between efficiency and accuracy.
21. In a higher dimensional double-integrator domain with obstacles, the two algs here SCE-RRT*, and TCE-RRT* outperform RRT*.
22. Use domain I havent heard of which is a weird version of a plane that has dubins steering in x,y and double integrator in z.

Sparse Roadmap Spanners. Dobson, Krontiris, Bekris. Workshop on the Algorithmic Foundations of Robotics (WAFR) 2012

1. PRMs approach optimality at the limit, but of course this is not actually practical in terms of time or memory requirements
2. If optimality is relaxed, near-optimal roadmaps can be made that are much more sparse
3. The approach here asymptotically:
   1. Is complete
   2. Near optimal
   3. Probability of adding nodes converges to zero as time passes
4. The implication is that finite size data structures can plan near optimally
5. Empirical results back these claims up
6. RRT isn’t focused on here, even though it can plan in dynamic domains because the goal is on preprocessing with PRMs
7. fixed k-PRM, where each point is connected to a fixed k number of nearest points isn’t optimal, but if kis scaled logarithmically with the number of nodes, it is.  Delta-PRM, which connects all points in a delta-ball is asymptotically optimal, I suppose because the number of points in the ball increases as the number of samples increases
   1. These methods just keep adding nodes and vertices at each iteration, and there is no way to know when to stop, so the graph simply grows and grows
8. The algorithm sequential roadmap spanner produces smaller maps by utilizing graph spanners that are subgraphs that have a node-to-node traversal cost that is allowed to be within some multiplicative factor of the cost in the full graph – therefore the graph remains small but the suboptimality of it is bounded
   1. In practice the graphs tend to be much smaller but don’t sacrifice much in terms of optimality
9. This paper introduces ther SPArse Roadmap Spanner algorithm.  It has the properties:
   1. Of being probabalistically complete
   2. Can connect any 2 pts of length tc*+4delta.  t and delta are parameters to the algorithm, c* is the cost of the optimum path between query points in Cfree
   3. Converges to a finite size roadmap
10. It is said to the best of their knowledge that this is the first approach that is near optimal and produces compact results – I wonder how Brafman’s paper from ICAPs compares?
11. The approach has a natural stopping criteria (not sure what it is yet though)
12. The alg builds 2 graphs in parallel, one sparse and one dense
    1. there are limits on edge lengths – the sparse graph has longer edges
13. Uses visibility based navigation through the sparse graphs with guard and bridge nodes
14. This method has a tiny fraction of the number of nodes delta-prm makes, and the path quality is very close