Perception, Planning, and Control

© Russ Tedrake, 2020-2022

Last modified .

How to cite these notes, use annotations, and give feedback.

**Note:** These are working notes used for a course being taught
at MIT. They will be updated throughout the Fall 2022 semester.

Previous Chapter | Table of contents | Next Chapter |

There are a few more essential skills that we need in our toolbox. In this chapter, we will explore some of the powerful methods of kinematic trajectory motion planning.

I'm actually almost proud of making it this far into the notes without
covering this topic yet. Writing a relatively simple script for the pose of
the gripper, like we did in the bin picking chapter, really can solve a lot of
interesting problems. But there are a number of reasons that we might want a
more automated solution:

- When the environment becomes more cluttered, it is harder to write such a simple solution, and we might have to worry about collisions between the arm and the environment as well as the gripper and the environment.
- If we are doing "mobile manipulation" -- our robotic arms are attached to a mobile base -- then the robot might have to operate in many different environments. Even if the workspace is not geometrically complicated, it might still be different enough each time we reach that it requires automated (but possibly still simple) planning.
- If the robot is operating in a simple known environment all day long, then it probably makes sense to optimize the trajectories that it is executing; we can often speed up the manipulation process significantly.

In fact, if you ran the clutter clearing demo, I would say that motion planning failures were the biggest limitation of that solution so far: the hand or objects could sometimes collide with the cameras or bins, or the differential-inverse kinematics strategy (which effectively ignored the joint angles) would sometime cause the robot to fold in on itself. In this chapter we'll develop the tools to make that much better!

I do need to make one important caveat. For motion planning in manipulation, lots of emphasis is placed on the problem of avoiding collisions. Despite having done some work in this field myself, I actually really dislike the problem formulation of collision-free motion planning. I think that on the whole, robots are too afraid of bumping into the world (because things still go wrong when they do). I don't think humans are solving these complex geometric problems every time we reach... even when we are reaching in dense clutter. I actually suspect that we are very bad at solving them. I would much rather see robots that perform well even with very coarse / approximate plans for moving through a cluttered environment, that are not afraid to make incidental contacts, and that can still accomplish the task when they do!

The goal of this chapter is to solve for motion trajectories. But I would argue that if you really understand how to solve inverse kinematics, then you've got most of what you need to plan trajectories.

We know that the forward kinematics give us a (nonlinear) mapping from joint angles to e.g. the pose of the gripper: $X^G = f_{kin}(q)$. So, naturally, one would think that the problem of inverse kinematics (IK) is about solving for the inverse map, $q = f^{-1}_{kin}(X^G).$ But, like we did with differential inverse kinematics, I'd like to think about inverse kinematics as the more general problem of finding joint angles subject to a rich library of costs and constraints; and the space of possible kinematic constraints is indeed rich.

For example, when we were evaluating grasp candidates for bin picking, we had only a soft preference on the orientation of the hand relative to some antipodal grasp. In that case, specifying 6 DOF pose of the gripper and finding one set of joint angles which satisfies it exactly would have been an overly constrained specification. I would say that it's rare that we have only end-effector pose constraints to reason about, we almost always have costs or constraints in joint space (like joint limits) and others in Cartesian space (like non-penetration constraints).

With its obvious importance in robotics, you probably won't be
surprised to hear that there is an extensive literature on inverse
kinematics. But you may be surprised at how extensive and complete the
solutions can get. The forward kinematics, $f_{kin}$, is a nonlinear
function in general, but it is a very structured one. In fact, with rare
exceptions (like if your robot has a helical
joint, aka screw joint), the equations governing the valid Cartesian
positions of our robots are actually *polynomial*. "But wait! What
about all of those sines and cosines in my kinematic equations?" you say.
The trigonometric terms come when we want to relate joint angles with
Cartesian coordinates. In $\Re^3$, for two points, $A$ and $B$, on the
same rigid body, the (squared) distance between them, $\|p^A - p^B\|^2,$
is a constant. And a joint is just a polynomial constraint between
positions on adjoining bodies, e.g. that they occupy the same point in
Cartesian space. See

Understanding the solutions to polynomial equations is the subject of
algebraic geometry. There is a deep literature on kinematics theory, on
symbolic algorithms, and on numerical algorithms. For even very complex
kinematic topologies, such as four-bar
linkages and Stewart-Gough
platforms, we can count the number of solutions, and/or understand
the continuous manifold of solutions. For instance,

While the algebraic-geometry methods are mostly targeted for offline
global analysis, they are not designed for fast real-time inverse
kinematics solutions needed in a control loop. The most popular tool
these days for real-time inverse kinematics for six- or seven-DOF
manipulators is a tool called "IKFast", described in Section 4.1
of

These explicit solutions are important to understand because they provide deep insight into the equations, and because they can be fast enough to use inside a more sophisticated solution approach. But the solutions don't provide the rich specification I advocated for above; in particular, they break down once we have inequality constraints instead of equality constraints. For those richer specifications, we will turn to optimization.

Rather than formulate inverse kinematics as $$q = f^{-1}_{kin}(X^G),$$ let's consider solving the same problem as an optimization: \begin{align} \min_q & \quad |q - q_0|^2, \\ \subjto &\quad X^G = f_{kin}(q), \end{align} where $q_0$ is some comfortable nominal position. While writing the inverse directly is a bit problematic, especially because we sometimes have multiple (even infinite) solutions or no solutions. This optimization formulation is slightly more precise -- if we have multiple joint angles which achieve the same end-effector position, then we prefer the one that is closest to the nominal joint positions. But the real value of switching to the optimization perspective of the problem is that it allows us to connect to a rich library of additional costs and constraints.

We have already
discussed the idea of solving *differential* inverse kinematics
as an optimization problem. In that workflow, we started by using the
pseudo-inverse of the kinematic Jacobian, but then graduated to thinking
about the least-squares formulation of the inverse problem. The more
general least-squares setting, we could add additional costs and
constraints that would protect us from (nearly) singular Jacobians and
could take into account additional constraints from joint limits, joint
velocity limits, etc. We could even add collision avoidance constraints.
Some of these constraints are quite nonlinear / nonconvex functions of
the configuration $q$, but in the differential kinematics setting we were
only seeking to find a small change $\Delta q$ around the nominal
configuration, so it was quite reasonable to make linear/convex
approximations of these nonlinear/nonconvex constraints.

Now we will consider the full formulation, where we try to solve the nonlinear / nonconvex optimization directly, without any constraints on only making a small change to an initial $q$. This is a much harder problem computationally. Using powerful nonlinear optimization solvers like SNOPT, we are often able to solve the problems, even at interactive rates (the example below is quite fun). But there are no guarantees. It could be that a solution exists even if the solver returns "infeasible".

Of course, the differential IK problem and the full IK problem are closely related. In fact, you can think about the differential IK algorithm as doing one step of (projected) gradient descent or one-step of Sequential Quadratic Programming, for the full nonlinear problem.

Drake provides a nice InverseKinematics
class that makes it easy to assemble many of the standard
kinematic/multibody constraints into a
`MathematicalProgram`

. Take a minute to look at the
constraints that are offered. You can add constraints on the relative
position and/or orientation on two bodies, or that two bodies are more
than some minimal distance apart (e.g. for non-penetration) or closer
than some distance, and more. This is the way that I want you to think
about the IK problem; it is an inverse problem, but one with a
potentially very rich set of costs and constraints.

Despite the nonconvexity of the problem and nontrivial computational cost of evaluating the constraints, we can often solve it at interactive rates. I've assembled a few examples of this in the chapter notebook:

In the first version, I've added sliders to let you control the desired pose of the end-effector. This is the simple version of the IK problem, amenable to more explicit solutions, but we nevertheless solve it with our full nonlinear optimization IK engine (and it does include joint limit constraints). This demo won't look too different from the very first example in the notes, where you used teleop to command the robot to pick up the red brick. In fact, differential IK offers a fine solution to this problem, too.

In the second example, I've tried to highlight the differences between the nonlinear IK problem and the differential IK problem by adding an obstacle directly in front of the robot. Because both our differential IK and IK formulations are able to consume the collision-avoidance constraints, both solutions will try to prevent you from crashing the arm into the post. But if you move the target end-effector position from one side of the post to the other, the full IK solver can switch over to a new solution with the arm on the other side of the post, but the differential IK will never be able to make that leap (it will stay on the first side of the post, refusing to allow a collision).

With great power comes great responsibility. The inverse kinematics toolbox allows you to formulate complex optimizations, but your success with solving them will depend partially on how thoughtful you are about choosing your costs and constraints. My basic advice is this:

- Try to keep the objective (costs) simple; I typically only use the "joint-centering" quadratic cost on $q$. Putting terms that should be constraints into the cost as penalties leads to lots of cost-function tuning, which can be a nasty business.
- Write minimal constraints. You want the set of feasible configurations to be as big as possible. For instance, if you don't need to fully constrain the orientation of the gripper, then don't do it.

Let's use IK to grasp a cylinder (call it a hand rail). Suppose it doesn't matter where along the cylinder we grasp, nor the orientation at which we grasp it. Then we should write the IK problem using only the minimal version of those constraints.

In the notebook, I've coded up one version of this. I've put the cylinder's pose on the sliders now, so you can move it around the workspace, and watch how the IK solver decides to position the robot. In particular, if you move the cylinder in $\pm y$, you'll see that the robot doesn't try to follow... until the hand gets to the end of the cylinder. Very nice!

One could imagine multiple ways to implement that constraint. Here's how I did it:

```
ik.AddPositionConstraint(
frameB=gripper_frame, p_BQ=[0, 0.1, -0.02],
frameA=cylinder_frame, p_AQ_lower=[0, 0, -0.5], p_AQ_upper=[0, 0, 0.5])
ik.AddPositionConstraint(
frameB=gripper_frame, p_BQ=[0, 0.1, 0.02],
frameA=cylinder_frame, p_AQ_lower=[0, 0, -0.5], p_AQ_upper=[0, 0, 0.5])
```

In words, I've defined two points in the gripper frame; in the notation
of the `AddPositionConstraint`

method they are ${}^Bp^{Q}$.
Recall the gripper frame is such
that $[0, .1, 0]$ is right between the two gripper pads; you should
take a moment to make sure you understand where $[0,.1,-0.02]$ and
$[0,.1,0.02]$ are. Our constraints require that both of those points
should lie exactly on the center line segment of the cylinder. This
was a compact way for me to leave the orientation around the cylinder
axis as unconstrained, and capture the cylinder position constraints
all quite nicely.
We've provided a rich language of constraints for specifying IK problems, including many which involve the kinematics of the robot and the geometry of the robot and the world (e.g., the minimum-distance constraints). Let's take a moment to appreciate the geometric puzzle that we are asking the optimizer to solve.

Let's return to the example of the iiwa reaching into the shelf. This IK problem has two major constraints: 1) we want the center of the target sphere to be in the center of the gripper, and 2) we want the arm to avoid collisions with the shelves. In order to plot these constraints, I've frozen three of the joints on the iiwa, leaving only the three corresponding motion in the $x-z$ plane.

To visualize the constraints, I've sampled a dense grid in the three joint angles of the planarized iiwa, assigning each grid element to 1 if the constraint is satisfied or 0 otherwise, then run a marching cubes algorithm to extract an approximation of the true 3D geometry of this constraint in the configuration space. The "grasp the sphere" constraint produces the nice green geometry I've pictured above on the right; it is clipped by the joint limits. The collision-avoidance constraint, on the other hand, is quite a bit more complicated. To see that, you'd better open up this 3D visualization so you can navigate around it yourself. Scary!

To help you appreciate the problem that we've formulated, I've made a visualization of the optimization landscape. Take a look at the landscape here first; this is only plotting a small region around the returned solution. You can use the Meshcat controls to show/hide each of the individual costs and constraints, to make sure you understand.

As recommended, I've kept the cost landscape (the *green*
surface) to be simply the quadratic joint-centering cost. The
constraints are plotted in *blue* when they are feasible, and
*red* when they are infeasible:

- The joint limit constraint is just a simple "bounding-box" constraint here (only the red infeasible region is drawn for bounding box constraints, to avoid making the visualization too cluttered).
- The position constraint has three elements: for the $x$, $y$, and $z$ positions of the end-effector. The $y$ position constraint is trivially satisfied (all blue) because the manipulator only has the joints that move in the $x-z$ plane. The other two look all red, but if you turn off the $y$ visualization, you can see two small strips of blue in each. That's the conditions in our tight position constraint.
- But it's the "minimum-distance" (non-collision) constraint that is the most impressive / scary of all. While we visualized the configuration space above, you can see here that visualizing the distance as a real-valued function reveals the optimization landscape that we give to the solver.

You should also take a quick look at the full optimization landscape. This is the same set of curves as in the visualization above, but now it's plotted over the entire domain of joint angles within the joint limits. Nonlinear optimizers like SNOPT can be pretty amazing sometimes!

For unconstrained inverse kinematics with exactly six degrees of freedom, we have closed-form solutions. For the generalized inverse kinematics problem with rich costs and constraints, we've got a nonlinear optimization problem that works well in practice but is subject to local minima (and therefore can fail to find a feasible solution if it exists). If we give up on solving the optimization problem at interactive rates, is there any hope of solving the richer IK formulation robustly? Ideally to global optimality?

This is actually and extremely relevant question. There are many applications of inverse kinematics that work offline and don't have any strict timing requirements. Imagine if you wanted to generate training data to train a neural network to learn your inverse kinematics; this would be a perfect application for global IK. Or if you want to do workspace analysis to see if the robot can reach all of the places it needs to reach in the workspace that you're designing for it, then you'd like to use global IK. Some of the motion planning strategies that we'll study below will also separate their computation into an offline "building" phase to make the online "query" phase much faster.

In my experience, general-purpose global nonlinear solvers -- for instance, based on mixed-integer nonlinear programming (MINLP) approaches or the interval arithmetic used in satisfiability-modulo-theories (SMT) solvers -- typically don't scale the complexity of a full manipulator. But if we only slightly restrict the class of costs and constraints that we support, then we can begin to make progress.

Drake provide an implementation of the GlobalInverseKinematics
approach described in

When should we use IK vs Differential IK? IK solves a more global problem, but is not guaranteed to succeed. It is also not guaranteed to make small changes to $q$ as you make small changes in the cost/constraints; so you might end up sending large $\Delta q$ commands to your robot. Use IK when you need to solve the more global problem, and the trajectory optimization algorithms we produce in the next section are the natural extension to producing actual $q$ trajectories. Differential IK works extremely well for incremental motions -- for instance if you are able to design smooth commands in end-effector space and simply track them in joint space.

In our first version of grasp selection using sampling, we put an objective that rewarded grasps that were oriented with the hand grasping from above the object. This was a (sometimes poor) surrogate for the problem that we really wanted to solve: we want the grasp to be achievable given a "comfortable" position of the robot. So a simple and natural extension of our grasp scoring metric would be to solve an inverse kinematics problem for the grasp candidate, and instead of putting costs on the end-effector orientation, we can use the joint-centering cost directly as our objective. Furthermore, if the IK problem returns infeasible, we should reject the sample.

There is a quite nice extension of this idea that becomes quite
natural once we take the optimization view, and it is a nice transition
to the trajectory planning we'll do in the next section. Imagine if the
task requires us not only to pick up the object in clutter, but also to
place the object carefully in clutter as well. In this case, a good grasp
is finds a pose for the hand relative to the object that
*simultaneously optimizes* both the pick configuration and the place
configuration. This is actually quite natural to do with inverse
kinematics; one can formulate an optimization problem with decision
variables for both $q_{pick}$ and $q_{place}$, with constraints enforcing
that ${}^OX^{G_{pick}} = {}^OX^{G_{place}}$.

Once we add in all of our other rich costs and constraints, this becomes a quite sophisticated approach. In the dish-loading project at TRI, this approach proved to be very important. Both picking up a mug in the sink and placing it in the dishwasher rack are highly constrained, so we needed the simultaneous optimization to find successful grasps.

Once you understand the optimization perspective of inverse kinematics, then you are well on your way to understanding kinematic trajectory optimization. Rather than solving multiple inverse kinematics problems independently, the basic idea now is to solve for a sequence of joint angles in the same optimization. Even better, let us define a parameterized joint trajectory, $q_\alpha(t)$, where $\alpha$ are parameters. Then a simple extension to our inverse kinematics problem would be to write something like \begin{align} \min_{\alpha,T} & \quad T, \\ \subjto &\quad X^{G_{start}} = f_{kin}(q_\alpha(0)),\\ & \quad X^{G_{goal}} = f_{kin}(q_\alpha(T)), \\ & \quad \forall t , \quad \left|\dot{q}_\alpha(t)\right| \le v_{max} \label{eq:vel_limits}. \end{align} I read this as "find a trajectory, $q(t)$ for $t \in [0, T]$, that moves the gripper from the start to the goal in minimum time".

The last equation, (\ref{eq:vel_limits}), represents velocity limits; this is the only way we are telling the optimizer that the robot cannot teleport instantaneously from the start to the goal. Apart from this line which looks a little non-standard, it is almost exactly like solving two inverse kinematics problems jointly, except instead of having the solver take gradients with respect to $q$, we will take gradients with respect to $\alpha$. This is easily accomplished using the chain rule.

The interesting question, then, becomes how to do we actually parameterize the trajectory $q(t)$ with a parameter vector $\alpha$? These says, you might think that $q_\alpha(t)$ could be a neural network that takes $t$ as an input, offers $q$ as an output, and uses $\alpha$ to represent the weights and biases of the network. Of course you could, but for inputs with a scalar input like this, we often take much simpler and sparser parameterizations, often based on polynomials.

There are many ways one can parameterize a trajectory with
polynomials. For example in *dynamic* motion planning, direct
collocation methods uses piecewise-cubic
polynomials to represent the state trajectory, and the pseudo-spectral
methods use Lagrange polynomials. In each case, the choice of basis
functions is made so that algorithm can leverage a particular property of
the basis. In dynamic motion planning, a great deal of focus is on the integration accuracy of the dynamic equations to ensure that we obtain feasible solutions to the dynamic constraints..

When we are planning the motions of our fully-actuated robot arms, we
typically worry less about dynamic feasibility, and focus instead on the
kinematics. For *kinematic* trajectory optimization, the so-called
B-spline
trajectory parameterization has a few particularly nice properties
that we can leverage here:

- The derivative of a B-spline is still a B-spline (of one less degree), with coefficients that are linear in the original coefficients.
- The bases themselves are non-negative and sparse.
This gives the coefficients of the B-spline polynomial, which are
referred to as the
*control points*, a strong geometric interpretation. - In particular, the entire trajectory is guaranteed to lie inside the convex hull of the active control points (the control points who's bases are not zero).

Note that B-splines are closely related to Bézier curves. But the "B" in "B-spline" actually just stands for "basis" (no, I'm not kidding) and "Bézier-splines" are slightly different.

The default `KinematicTrajectoryOptimization`

class in Drake optimizes a trajectory defined using a B-spline to
represent a path, $r(s)$ over the interval $s \in [0,1]$, plus an
additional scalar decision variable corresponding to the trajectory
duration, $T$. The final trajectory combines the path with the
time-rescaling: $q(t) = r(t/T).$ This is a particularly nice way to
represent a trajectory of unknown duration, and has the excellent feature
that the convex hull property can still be used. Velocity constraints are
still linear; constraints on acceleration and higher derivatives do
become nonlinear, but if satisfied they still imply strict bounds
$\forall t \in [0, T].$

Since the `KinematicTrajectoryOptimization`

is written
using Drake's `MathematicalProgram`

, by default it will
automatically select what we think is the best solver given the available
solvers. If the optimization has only convex costs and constraints, it
will be dispatched to a convex optimization solver. But most often we add
in nonconvex costs
and constraints from kinematics. Therefore in most cases, the default
solver would again be the SQP-solver, SNOPT. You are free to experiment
with others!

One of the most interesting set of constraints that we can add to our
kinematic trajectory optimization problem is the MinimumDistanceConstraint;
when the minimum distance between all potential collision pairs is
greater than zero then we have avoided collisions. Please note, though,
that these collision constraints can only be enforced at discrete
samples, $s_i \in [0,1]$, along the path. *They do not guarantee that
the trajectory is collision free $\forall t\in[0,T].$* It required
very special properties of the derivative constraints to leverage the
convex hull property; we do not have them for more general nonlinear
constraints. A common strategy is to add constraints at some modest
number of samples along the interval during optimization, then to check
for collisions more densely on the optimized trajectory before executing
it on the robot.

As a warm-up, I've provided a simple example of the planar iiwa reaching from the top shelf into the middle shelf.

If you look carefully at the code, I actually had to solve this trajectory optimization twice to get SNOPT to return a good solution (unfortunately, since it is a local optimization, this can happen). For this particular problem, the strategy that worked was to solve once without the collision avoidance constraint, and then use that trajectory as an initial guess for the problem with the collision avoidance constraint.

Another thing to notice in the code is the "visualization callback" that I use to plot a little blue line for the trajectory as the optimizer is solving. Visualization callbacks are implemented by e.g. telling the solver about a cost function that depends on all of the variables, and always returns zero cost; they get called every time the solver evaluates the cost functions. What I've done here can definitely slow down the optimization, but it's an excellent way to get some intuition about when the solver is "struggling" to numerically solve a problem. I think that the people / papers in this field with the fastest and most robust solvers are highly correlated with people who spend time visualizing and massaging the numerics of their solvers.

We can use `KinematicTrajectoryOptimization`

to do the
planning for our clutter clearing example, too. This optimization was
more robust, and did not require solving twice. I only seeded it with a
trivial initial trajectory to avoid solutions where the robot tried to
rotate 270 degrees around its base instead of taking the shorter
path.

There are a number of related approaches to kinematic trajectory
optimization in the motion planning literature, which differ either by
their parameterization or by the solution technique (or both). Some of
the more well-known include CHOMP

KOMO, for instance, is one of a wave of trajectory optimization
techniques that use the Augmented
Lagrangian method of transcribing a constrained optimization problem
into an unconstrained problem, then using a simple but fast
gradient-based solver

When kinematic trajectory optimizations succeed, they are an incredibly satisfying solution to the motion planning problem. They give a very natural and expressive language for specifying the desired motion (with needing to sample nonlinear constraints as perhaps the one exception), and they can be solved fast enough for online planning. The only problem is: they don't always succeed. Because they are based on nonconvex optimization, they are susceptible to local minima, and can fail to find a feasible path even when one exists.

Consider the extremely simple example of finding the shortest path
from the start (blue star) to the goal (green star) in the image above,
avoiding collisions with the red box. Avoiding even the complexity of
B-splines, we can write an extremely simple optimization of the form:
\begin{aligned} \min_{q_0, ..., q_N} \quad & \sum_{n=0}^{N-1} | q_{n+1}
- q_n|_2^2 & \\ \text{subject to} \quad & q_0 = q_{start} \\ & q_N =
q_{goal} \\ & |q_n|_1 \ge 1 & \forall n, \end{aligned} where the last
line is the collision-avoidance constraint saying that each sample
point has to be *outside* of the $\ell_1$-ball.

Once a nonlinear solver is considering paths that go right around the obstacle, it is very unlikely to find a solution that goes left around the obstacle, because the solution would have to get worse (violate the collision constraint) before it gets better.

To deal with this limitation, the field of collision-free motion planning has trended heavily towards sampling-based methods.

The Open Motion Planning Library. We have our own implementations in Drake that are optimized for our collision engines.

For this exercise, you will implement a optimization-based inverse kinematics solver to open a cupboard door. You will work exclusively in . You will be asked to complete the following steps:

- Write down the constraints on the IK problem of grabbing a cupboard handle.
- Formalize the IK problem as an instance of optimization.
- Implement the optimization problem using MathematicalProgram.

For this exercise, you will implement and analyze the RRT algorithm introduced in class. You will work exclusively in . You will be asked to complete the following steps:

- Implement the RRT algorithm for the Kuka arm.
- Answer questions regarding its properties.

Due to the random process by which nodes are generated, the paths output by RRT can often look somewhat jerky (the "RRT dance" is the favorite dance move of many roboticists). There are many strategies to improve the quality of the paths and in this question we'll explore two. For the sake of this problem, we'll assume path quality refers to path length, i.e. that the goal is to find the shortest possible path, where distance is measured as Euclidean distance in joint space.

- One strategy to improve path quality is to post-process paths via "shortcutting", which tries to replace existing portions of a path with shorter segments
Geraerts04 . This is often implemented with the following algorithm: 1) Randomly select two non-consecutive nodes along the path. 2) Try to connect them with a RRT's extend operator. 3) If the resulting path is better, replace the existing path with the new, better path. Steps 1-3 are repeated until a termination condition (often a finite number of steps or time). For this problem, we can assume that the extend operator is a straight line in joint space. Consider the graph below, where RRT has found a rather jerky path from $q_{start}$ to $q_{goal}$. There is an obstacle (shown in red) and $q_{start}$ and $q_{goal}$ are highlighted in blue (disclaimer: This graph was manually generated to serve as an illustrative example). Name one pair of nodes for which the shortcutting algorithm would result in a shorter path (i.e. two nodes along our current solution path for which we could produce a shorter path if we were to directly connect them). You should assume the distance metric is the 2D Euclidean distance. - Shortcutting as a post-processing technique, reasons over the existing path and enables local "re-wiring" of the graph. It is a heuristic and does not, however, guarantee that the tree will encode the shortest path. To explore this, let's zoom in one one iteration of RRT (as illustrated below), where $q_{sample}$ is the randomly generated configuration, $q_{near}$ was the closest node on the existing tree and $q_{new}$ is the RRT extension step from $q_{near}$ in the direction of q_sample. When the standard RRT algorithm (which you implemented in a previous exercise) adds $q_{new}$ to the tree, what node is its parent? If we wanted our tree to encode the shortest path from the starting node, $q_{start}$, to each node in the tree, what node should be the parent node of $q_{new}$?

For this exercise, you will implement part of the IRIS algorithm

- Implement a QP that finds the closest point on an obstacle to an ellipse in free-space.
- Implement the part of the algorithm that searches for a set of hyperplanes that separate a free-space ellipse from all the obstacles.

- "An Architecture for Online Affordance-based Perception and Whole-body Planning", Journal of Field Robotics, vol. 32, no. 2, pp. 229-254, September, 2014. [ link ] ,
- "Director: A User Interface Designed for Robot Operation With Shared Autonomy", Journal of Field Robotics, vol. 1556-4967, 2016. [ link ] ,
- "Numerical algebraic geometry and algebraic kinematics", Acta Numerica, vol. 20, pp. 469-567, 2011. ,
- "Automated Construction of Robotic Manipulation Programs", PhD thesis, Carnegie Mellon University, August, 2010. ,
- "Global inverse kinematics via mixed-integer convex optimization", International Symposium on Robotics Research, 2017. [ link ] ,
- "Globally optimal solution to inverse kinematics of 7DOF serial manipulator", IEEE Robotics and Automation Letters, vol. 7, no. 3, pp. 6012--6019, 2022. ,
- "{CHOMP}: Gradient Optimization Techniques for Efficient Motion Planning", IEEE International Conference on Robotics and Automation (ICRA) , May, 2009. ,
- "{STOMP}: Stochastic trajectory optimization for motion planning", 2011 IEEE international conference on robotics and automation , pp. 4569--4574, 2011. ,
- "A tutorial on Newton methods for constrained trajectory optimization and relations to SLAM, Gaussian Process smoothing, optimal control, and probabilistic inference", Geometric and numerical foundations of movements, pp. 361--392, 2017. ,
- "A Novel Augmented Lagrangian Approach for Inequalities and Convergent Any-Time Non-Central Updates", , 2014. ,
- "A comparative study of probabilistic roadmap planners", Algorithmic Foundations of Robotics V, pp. 43--58, 2004. ,
- "Sampling-based Algorithms for Optimal Motion Planning", Int. Journal of Robotics Research, vol. 30, pp. 846--894, June, 2011. ,
- "Computing Large Convex Regions of Obstacle-Free Space through Semidefinite Programming", Proceedings of the Eleventh International Workshop on the Algorithmic Foundations of Robotics (WAFR 2014) , 2014. [ link ] ,

Previous Chapter | Table of contents | Next Chapter |