MEAM 620: Robotics

Finding Optimal C-Space Paths By Variation of Curves

Abstract

The goal of this project is to find paths through configuration space for a 2-DOF planar manipulator that are "optimal" in terms of some abstract cost definition such as path length or energy consumption, while avoiding obstacles. We propose a two-step planner which finds feasible paths using RRT and then optimizes them using a variation of curves technique from differential geometry.

Introduction and Related Work

The task of planning for a mobile manipulator can be reduced to finding paths through its configuration space.

Some obstacles in configuration space are created by physical workspace obstacles while others implicitly appear because of constraints on joint torques for the manipulator. Large end-effector loads may result in large joint torques depending on the joint configuration, and constraints on the former result in regions of C-space being rendered impassable (for known, fixed load).

A key consideration is the trade-off that needs to be made between guaranteeing global optimality and scalability of the planning algorithm. An algorithm such as RRT (1) can be used to quickly and efficiently find a feasible (but suboptimal) path through C-Space. Current techniques that guarantee global optimality (such as discretization followed by A* graph search) are more computationally expensive and often unusable for systems of dimension higher than 4.

The method proposed here is to use a two-step process, whereby we find a feasible initial path, and vary it till we reach a path that is in the same homotopy group as the initial path but has a shorter path. It should be noted here that the homotopy equivalence means that the path obtained may not be a global optimum, but if our variation technique is "good" it will be optimum in that homotopy class of paths.

The idea of using a two-step planning approach is not new in the literature, but the use of RRT in combination with variation of curves seems to not be very common (at best). There has recently been work on a tree version of RRT called RRT* (4) which has probabilistic guarantees of getting to the optimal path, but remains a sampling based technique and does not make it possible to smooth the trajectory at all. Meanwhile (3) presents a gradient descent approach to cost minimization, but they require an analytic cost functional. In particular, in the results they report finding a collision free path in most (but not all) simulation problems. Our objective here is to guarantee finding a feasible path, but optimize using continuous curve deformation techniques.

Problem Setting and Notation

To develop our tools, we restrict ourselves to the specific case of a 2-DOF planar manipulator with workspace obstacles that are discs on this plane (for efficient computation of C-Space obstacles).

As marked in the figure, our joint variable is a vector q which is an element of the two torus T^2 . In configuration space, we expect to see some embedding of the two torus; we will use a square with opposite edges identified with proper orientation.

Configuration Space Obstacles

As stated before, it is easier to perform planning in configuration space because the problem gets reduced to finding a feasible path for a particle in this space. The following figure shows the workspace configuration above in configuration space.

Obstructions from Workspace Obstacles

Writing the forward kinematics of this robot is possible just using trigonometry, but for non-planar systems an approach using twists would be favorable (and equally tractable).

For joint configuration q=(q_1, q_2)^T, let us denote the workspace positions of the two links as a_1, a_2 for the proximal and distal points of the first link, b_1, b_2 for the second link. These are all points in R^2.

Note

• a_1(q) = 0,
• a_2(q) = b_1(q) = (l_1 \cos q_1, l_1 \sin q_1), and
• b_2(q) = (l_1 \cos q_1 + l_2 \cos(q_1+q_2), l_1 \sin q_1 + l_2 \sin(q_1+q_2)).

To check for obstruction, we must ensure that every point in the closed line segment between a_1 and a_2 (similarly b_1 and b_2) lies outside each obstacle. Using algebraic geometry we can define the interior of the disc as

||x(µ)-p_j||^2 < r_j^2.

So, we much check for each link i and each obstacle j that

x(µ):=µ a_i(q) + (1-µ) b_i(q)

has a solution for µ in the interval [0,1] for

r_j^2 - ||x(µ)-p_j||^2 < 0,

where obstacle j is a disc with center p_j and radius r_j.

Obstructions from Joint Torque Constraints

Suppose the end effector position is given by the function p=g(q) (we already computed this as b_2(q) in the last section). Then, we can easily show that forces on the end effector W can be related back to joint torques τ as

We want our joint torques to be under certain limits,

For a fixed load, this kind of constraint results in almost analytic constraints on each joint, but if we have a fixed joint torque limit, the max joint torque is obviously not a smooth function over the C-Space.

Step 1: Finding a Feasible Path using RRT

The Rapidly-exploring Random Trees (RRTs) algorithm (1) is a well-known existing algorithm that samples points in configuration space and attempts to find a feasible path that may be far from optimal.

The only difficulty here is that the particular embedding of the two torus needs to have a distance function defined on it that takes care of the identified edges.

Step 2: Decreasing Cost of Feasible Path using Variation of Curves

In this step, we assume we start from a feasible curve. The overall idea of varying curves is to find a "descent direction" in the infinite dimensional space of curves in the same homotopy group as the feasible path, and move in that direction.

For the specific case of a 2D configuration space, the cost manifold is a surface, and we can employ techniques from differential geometry. For a unit-speed curve γ(s) on a surface M, we can define an orthonormal coordinate frame {T(s), S(s), n(s)} at every point on the curve, where T(s) is the tangent vector, n(s) is the surface normal, and S(s) is on the tangent space but orthogonal to the tangent (2).

Intuitively, we need to move the curve in a direction (anti)parallel to S(s). It is not hard to show that if we move the curve in a direction v(s) defined as

that is, the length of the curve is instantaneously decreasing if the function λ(s) is chosen so that λ(s) κ_g(s) > 0.

This approach comes with a provable guarantee that the new path will have shorter length, but it requires computation of the geodesic curvature κ_g. This needs the curve (and the surface) to both be C^2. The parameterization of such a curve from implementation purposes is difficult, and numerical approximations with interpolation functions failed.

Using a Spline Parameterization

If we commit to a specific parameterization of the curve, such as splines, this method becomes easy to implement. However, with splines, we lose so much freedom on how we can "tweak" the curve that the same provable guarantees do not apply.

The intuitive idea is the same as before, we move the spline control points in the direction of geodesic curvature. In the iterative update process, we need to check if we are colliding with any non-analytic obstacles.

Results

We can apply all our ideas with a spline implementation to demonstrate planning for a 2-DOF manipulator.

More Simulation Results and Analysis

For each of the following the leftmost image shows the succession of variation of curves iterations in increasing opacity, overlaid on the configuration space. The next three images show the robot in workspace in three different configurations along the trajectory, starting with the initial (second image), and ending and the final (rightmost image) configuration.

Trial 2 showcases some of the drawbacks of using splines to construct the path. The initial waypoints obtained using RRT assumed "straight line" segments between the waypoints in the feasibility test. Fitting splines makes the initial path infeasible. Running the variation of curves results in obtaining a feasible path, but this may not generally always be the case. In particular, narrow areas in the configuration space (such as the region around the start point in this trial) are problematic for the spline fitting approach.

Trial 3 demonstrates the correct behavior using the redefined metric on the planar embedding of the two torus: the trajectory found "wraps around" the x-direction. This trial also strongly demonstrates how effective the variation of curves technique is in reducing the cost of the path (our cost here is just the length of the path in configuration space, but as mentioned before any arbitrary analytic cost function on the C-Space works fine), over just using RRT.

Trial 4 demonstrates firstly the limitation that this two-step method is not guaranteed to get even close to the global optimum. In this trial note that a shorter overall path may have been obtained by going between the obstacle regions marked A and B. However, the RRT method returned an initial path in a different homotopy group, and this path cannot be smoothly deformed into the global optimum.

Secondly, this trial demonstrates the ability of the variation of curves technique to "smooth" the initial spline trajectory that had sub-optimally high curvature around the penultimate control point.

Thirdly, we note that this path can be improved dramatically by increasing θ_2 for the second and third control points, but the variation of curves process stopped because of collision issues with the penultimate segment. This particular problem should be easily fixable by even a naive heuristic which checks which control points can be moved. One worries, though, that with long paths with a large number of waypoints there could be a combinatorial explosion in checking which control points can be moved.

Video

The following video shows the result of the planned path in Trial 1. The splines automatically result in a smooth path through C-Space, and because of the continuous nature of the forward kinematics vector field, the manipulator trajectory in the workspace also looks smooth.

http://www.youtube.com/watch?v=oXu_byLBLPE

Future Work

Some things to consider in the future are a clever(er) parameterization of a curve for implementing variation of curves. Splines are too simple: for ease of implementation we sacrifice so much freedom that we cannot prove anything.

Extension to higher dimensional manifolds is an obvious extension. Similar techniques will work, but note that there is more freedom in perturbation directions for the curve (in 2D we were restricted to vary in the direction of the intrinsic normal, but in a d-dimensional manifold, there are d-1 possible directions of perturbation.

A very useful but difficult upgrade to this project could be to form an analytic approximation of cost (which only needs to be valid locally). The idea is that instead of having obstacles in C-Space, we just find (local) shortest paths on an analytic cost manifold. With this method we could just start from a "straight line" in C-Space and execute our variation of curves. The problem is that we need to have an analytical representation of the surface (at least in a narrow band around the current path).

Acknowledgements

I would like to thank Professor Vijay Kumar for a helpful project outline and the important suggestion to commit to a parameterization (splines).

Bibliography

1. S. M. LaValle. Planning Algorithms. Cambridge University Press, Cambridge, U.K., 2006. Available at http://planning.cs.uiuc.edu/.
2. R. S. Millman, G. D. Parker. Elements of Differential Geometry. Prentice Hall, 1977.
3. N. Ratcliff, M. Zucker, J. A. Bagnell, S. Srinivasa. CHOMP: Gradient Optimization Techniques for Efficient Motion Planning, IEEE International Conference on Robotics and Automation (ICRA), May, 2009.
4. S. Karaman, E. Frazzoli. Incremental Sampling-based Algorithms for Optimal Motion Planning, RSS , May, 2010.

---

Final presentations

Anirudha Majumdar

• Your idea was very cool, and I found it really impressive the range of trajectories you got from SNOPT (especially the "wind-up" kick which arose out of you requesting a higher kick speed)
• Maybe you could store a set of "kick" trajectories, analogous to gaits for walking robots and pick one in order to avoid having the optimization.

Ben Charrow

• Impressive video, and very cool project. I don't envy having to get all of those robots to function as well as you did.
• You mentioned the "door grasping" is a feedworward trajectory. For the whole state machine it seems like the only part that needs feedback is the initial position of the PR2...some vision could turn this into autonomous door opening!

Brian MacAllister

• I appreciated that you explored improvements on A*, something that we used in class. Your video at the end really showcased the complexity of your project, including the limited range sensing which I didn't realize was an issue till the end.
• Your presentation could use some more figures towards the beginning, I was kind of confused till the end.

Katy Powers

• I really liked your presentation, and the simultaneous convergence of mass estimate and trajectory for your planar example was a pretty cool result! Your explanation of adaptive control in the presentation was very lucid and your "intuitive explanation" of the W matrix was very helpful.
• Does adaptation work with other controllers? I was wondering if a simple feedforward torque trajectory can be used to estimate mass. Maybe you can investigate a "minimal" set of torque trajectories required to get a good estimate.

Fei Miao

• I appreciate that you investigated A* graph search more and other planning methods, almost everyone in the class should benefit from your results.
• Your example at the end with the two robots in the narrow hallway should probably have been in the video...it would be really cool to see.

Jason Owens

• Backpack seems like a hard thing to detect, way to challenge yourself. I liked your explanation of HoG, and the video was impressive. I was surprised to see the viewpoint independent detection.
• Is there anything specific about the structure of a backpack that you can use? Backpacks are generally soft, diffuse reflectance?

Kartik Mohta

• Your presentation was great, nice explanation of (Probabilistic) Lloyd's algorithm.
• Some results of the difference between using centroidal vs regular voronoi would be nice.

Menglong Zhu

• That was a very impressive video, a talking PR2 is pretty funny! I also thought it was a really clever idea to modify edit distance based on letter appearance.
• What did you modify in the MS algorithm? Some details would be nice. For bright vs dark letters can you simply take the negative of the image?

Phil Dames

• I like the general idea of using gradient descent on information space...I think that may be way of the future for autonomous exploration. Good presentation, and nice use of RANSAC!
• I would have liked some details about mutual information? A comparison to randomly picking directions would be nice (for the same number of iterations).

Steve McGill

• Nice presentation, and your videos were really cool. Your project was very broad, including sensing, locomotion and coordination (most people only explored one of these areas)...I thought you did an amazing job!
• I said this in a question, but what about if one side of the table is much heavier? Maybe you could use torque sensors at the knee to detect table weight, and make the robots keep the table horizontal?

• Final Projects
• Anirudha Majumdar
• Avik De
• Ben Charrow
• Brandon Fischer
• Brian MacAllister
• Caitlin Powers
• Cem Karan
• Ceridwen Magee
• Chinwendu Enyioha
• Fei Miao
• Ian McMahon
• Jason Owens
• Kartik Mohta
• Menglong Zhu
• Monica Lui
• Philip Dames
• Shuai Li
• Steve McGill
• Teyvonia Thomas
• Yida Zhang