<jupyter_start><jupyter_text>Section III. MOTION PLANNING     Chapter 8. What is motion planning? With the tools developed so far, we have developed a good understandingof robots at static poses, but we have not yet developed anunderstanding of how they move. Later we will cover the topic ofdynamics and control, which will seek theories to model and regulate thevelocities, accelerations, and forces acting on the robot to generatemotion. But for the moment, we can still develop a great deal ofunderstanding about robot motion using the kinematics we have developedso far. In this section of the book, we will consider the central question ofmotion planning, loosely defined as follows.> **Motion planning**: the use of computational methods to generate a robot's motion to achieve a specified task.Motion planning is a critical component of any autonomous system that exhibits intelligent behavior.  It plays a central role in the ecosystem of components of a robot, since the inputs to a planner include the robot’s state (coming from localization), environmental conditions (coming from perception), dynamic predictive models (coming from calibration and simulation), and task specification (coming from systems engineers and human supervisors), and the output of a motion planner is a low-level motion representation (sent to the controller). In short, motion planning governs how _all of the knowledge_ available to the system influences _all actuation decisions_.*************************![fig:PlanningOverview](figures/planning/overview.svg)Figure 1.    The role of planning is to consume mission objectives, perception outputs, and prior information to generate a representation of the robot's motion.  The motion will be executed by the controller.*************************The three inputs into a motion planner are the system state, the system model and the task specification. For example, in the context of autonomous vehicles, the system state may specify the vehicle’s state, the map around it, and the state and intentions of other agents. The system model predicts how the system state changes in the future, conditioned on the robot’s decisions. The task specification includes cost function(s) that measure the quality of candidate motions as well as constraints that the motion must meet.  For example, an autonomous vehicle may wish to maximize ride comfort while also remaining collision-free, staying within a lane, etc. In Chapters [9](GeometricMotionPlanning.ipynb) and [10](MotionPlanningHigherDimensions.ipynb), we will focus mainly on the problem of *kinematic motion planning* (akapath planning), defined as follows:> **Kinematic motion planning** (aka **Path planning**): the use of computational methods generate a continuous path between two robot configurations while respecting joint limits and avoiding obstacles.This is purely a *geometric* problem that sidesteps the complexity of timing and dynamics. The resulting path is much like a route planned on a map, which instructs you the sequence of routes andturns to take to reach the goal, but not how quickly to drive. Although this doesn't solve the motion planning problem in its entirety, the resulting algorithms are still quite useful for robot manipulators and high-level routes for mobile robots. Indeed, the techniques studied for this simplified problems are often used in practice because it is relatively straightforward to construct schemes for some robots (e.g., industrialrobots and differential drive mobile robots) to track a geometric path. Also, by studying the kinematic problem we introduce computational representations, algorithms, and complexity theorems usedubiquitously in robot decision making.Highly dynamic robots like autonomous cars and agile drones will require reasoning about dynamics *upfront* during the planning procedure.  We will discuss how to plan for these systems in [Chapter 11](PlanningWithDynamicsAndUncertainty.ipynb). Later will revisit the problem ofdynamics and uncertainty in [Section IV](WhatAreDynamicsAndControl.ipynb).In the [final chapter](AdvancedTopicsInPlanning.ipynb) of this section we discuss specialized applications of planning to multi-agent systems, planning for manipulation and legged locomotion, and integrating motion planning with high-level reasoning. Why is planning hard?---------------------Because it is natural for humans to navigate their environments (atleast outside of hedge mazes!) the motion planning problem atfirst glance may appear to be somewhat straightforward. Our brains quitenaturally build mental maps of the space of possible movements whenlooking at obstacles around us. However, this spatial reasoning isnontrivial for robots, which must break down the navigation task into asequence of low-level actions.Initial attempts to solving this problem might look something like ascript: move forward toward the goal until it is reached or an obstacleis encountered; if an obstacle is reached, then move left, then turnright once the obstacle is cleared, and repeat from step 1, etc.Although scripting behaviors may work with very simple obstacles, thisapproach becomes fraught with brittleness once obstacles become morevaried or complex.More sophisticated *deliberative* reasoning is needed to successfullynavigate in more complex environments, like indoor navigation or outdoorterrain. In order to do so, the robot must be able to hypothesizevarious future states that it might be in, and actions it might take.Planning is somewhat like playing a game of chess, in that you imaginepossible future states of the board as a result of various actions thatyou might take. This cannot be done efficiently in a naive fashion. If$n$ steps were needed to reach a goal, and $m$ actions were available ateach step, then there would be $m^n$ possible future action sequences!This can rapidly become huge, even for small values of $m$ and $n$ (notto mention the fact that robots can perform a *continuously infinite*number of actions.) As a result, we must be more careful about how tostructure the deliberative reasoning process.Stating the motion planning problem-----------------------------------Let us assume the robot has configuration space (C-space) $\mathcal{C}$.A motion planning problem is specified with the following inputs:-   A kinematic and geometric description of the robot-   A geometric description of the robot's environment-   Constraints on how the robot is allowed to move-   The robot's start configuration-   A description of the robot's goal (usually a configuration, a point    in task-space, or a subset of configuration space)-   Optionally, an optimality criterion that should be minimized or maximized    (such as minimum path length,    maximum clearance from obstacles, or minimum energy expended)A *feasible path* is a continuous curve in C-space$y(s):[0,1]\rightarrow \mathcal{C}$ that does not violate anyconstraints. A *solution path* is a feasible path beginning at the startconfiguration and ending at the goal. If an optimality criterion isprovided, then a solution path should also be optimal (e.g., minimize acost function) or at least close to optimal.There are numerous types of motion constraints that may be specified,which can be categorized by the manner in which they constrain therobot's path:-   Local (kinematic): each point along the path must satisfy some    condition, e.g., avoid collision with obstacles.-   Differential (dynamic): the derivative along the path must satisfy    some condition, e.g., bounded curvature.-   Global: the entire path must satisfy some condition, e.g., path    length must be within some bound.When both kinematic and dynamic constraints are present, this is knownas a *kinodynamic planning* problem.*************************| (a)  Local (kinematic) | (b) Differential (dynamic) | Global ||------------------------|----------------------------|--------||![fig:PlanningConstraints1](figures/planning/constraints1.svg)|![fig:PlanningConstraints1](figures/planning/constraints2.svg)|![fig:PlanningConstraints1](figures/planning/constraints3.svg)|Figure 2.    Illustrating the three classes of constraints.  Local constraints include collisions with obstacles.  Differential constraints limit the derivative of the robot's path.  Global constraints are imposed on the entire path, e.g., the robot must return to its base before its battery is exhausted.************************* Point-to-point path planningIn the most basic path planning problem, which is the topic of most ofthe next chapters, we consider problems that are *point-to-point,feasible, and kinematic*. In other words, the motion constraints includeonly obstacle avoidance and joint limit avoidance, the robot's goal is asingle configuration, and there is no optimality criterion.In path planning we can speak of a subset of C-space that contains allconfigurations that do not violate any motion constraints. This is knownas the *free space* $\mathcal{F} \subseteq \mathcal{C}$ defined suchthat$$\mathcal{F} = \{ q \in \mathcal{C} \,|\, q_{min} \leq q \leq q_{max} \text{ and } R(q) \cap E = \emptyset \}.$$Here, $q_{min}$ and $q_{max}$ are the lower and upper joint limits,respectively, $R(q) \subset \mathbb{R}^2$ or $\mathbb{R}^3$ is thesubset of workspace occupied by the robot, and $E$ is the subset of theworkspace occupied by the environment.The complement of the free space is known as the *forbidden space*$\mathcal{C} \setminus \mathcal{F}$. Our problem can be restated asfinding a continuous curve $y(s)$ such that:-   $y(s) \in \mathcal{F}$ for all $s \in [0,1]$ (feasibility)-   $y(0) = q_I$ (initial configuration)-   $y(1) = q_G$ (goal configuration).It may be somewhat difficult for a computer to reason about all possible curves, since a curve is a collection of an infinite number of points.  Instead, it will be necessary to represent paths in a finite number of parameters.  We will describe various [path representations](Representations-of-paths) below. Completeness and optimalityA *motion planner* is an algorithm that solves a class of motionplanning problems. Motion planners are characterized by the class ofproblems addressed, as well as their performance in solving them.The first question to consider is that of *completeness*: "will theplanner find a path (if one exists)?" Specifically:> **Complete planner**. A planner is complete if it finds a path in finite time if one exists, *and* it> terminates with "no path" when no path exists.We shall see in the next section that it is difficult to develop complete planners outside of relatively simple scenarios.  A relaxed version ofcompleteness is *resolution-completeness*, in which the planner isguaranteed to find a path if one exists as some resolution parameter$h$, say, the size of a grid cell, approaches 0. However, if the plannerreturns "no path" for a given value of $h$, then we cannot tell whetherthere truly exists no path, or $h$ was not set small enough. > **Resolution-complete planner**. A planner is resolution-complete if it accepts a resolution parameter $h>0$, such that if a path exists, it will find one in finite time given a value of $h$ made sufficiently small.Another relaxed version is *probabilistic completeness*, in which the*likelihood* of failing to find a path, when one exists, approaches 0 asmore time is spent planning. If such a planner returns "no path" aftersome amount of time, then we cannot tell whether there truly exists nopath, or not enough time was spent planning to yield a high chance offinding a path.> **Probabilistically-complete planner**. A planner is probabilistically complete if, when a path exists, then the probability that it fails to find one decreases toward 0 as more time is spent planning.The other property to consider that of *optimality*: "will the plannerfind an optimal path?" > **Optimal planner**. An optimal planner terminates in finite time and computes theoptimal path, if one exists. Hence, optimal planners must also be complete, but the converse is not necessarily true. Given the difficulty of developing complete planners, we also consider relaxed versions of optimality.An *approximately-optimal* planner willterminate with a path whose cost is no more than $(1+\epsilon)$ timesthe optimal cost, where $\epsilon > 0$ is some constant. An*asymptotically-optimal* planner will, given enough time, find a pathwhose cost is within a factor of $(1+\epsilon)$ times the optimal costfor *any* specified value $\epsilon > 0$. In other words, given moretime, the planner will produce progressively better paths. Explicit vs implicit plans (paths vs policies)The classical form of planning described above _computes a single path_ (or declares "no path found"). The planned path is then sent to a controller (described in more detail in [Section IV](WhatAreDynamicsAndControl.ipynb)) which will then try to *execute* the plan by generating actuator commands to follow it.  Over the course of execution, the robot's state will trace out an *executed path*, which may or may not be close to the original plan.   For some systems and situations, everything goes "according to plan" and the robot will follow the path faithfully, and predictions about the world's behavior are accurate ([Fig. 3](fig:Execution)).  But in many settings, trusting the plan is risky.| (a)  Imperfect plan execution | (b) Imperfect prediction about other agents in the world|---------------------------------------------------------------|--------------------------------------------------------||![fig:ImperfectExecution](figures/planning/imperfect-execution.svg) | ![fig:ImperfectPrediction](figures/planning/imperfect-prediction.svg) |Figure 3.     A planned path may differ from an executed path.  (a) The robot's execution (dotted line) does not track the plan (solid line) well, triggering a replan that requires it to back up to pass through the obstacles.  (b) The robot executes its path perfectly, but its initial plan predicts that the pedestrian will continue walking (dotted line). When the pedestrian stops, the robot must detect that the plan is invalid, and take an alternate path.    *************We shall cover two methods for planning under imperfect execution / prediction in [Chapter 11](PlanningWithDynamicsAndUncertainty.ipynbPlanning-under-imperfect-execution).  The first idea addresses the problem at the level of the sense-plan-act loop by frequently *replanning*.  Replanning can happen either on a fixed schedule or when the robot deviates from the plan; if it can be done fast enough, then the robot can respond to changes in the environment before a catastrophic failure. The second idea is to modifying the planning problem so that it generates a _policy_, which is a sensor-based  process that is designed to respond to changes in the robot's state or the environment.  We will discuss policies in more detail in [Chapter 11](PlanningWithDynamicsAndUncertainty.ipynbMarkov-Decision-Processes-(MDPs)) and when we cover control in [Section IV](WhatAreDynamicsAndControl.ipynb).  To plan a policy, the robot must be able to anticipate when and how execution can go wrong, and determine contingency plans for all possible outcomes. Suffice to say, finding complete / optimal policies is at least as hard as finding complete / optimal paths.   Heuristic methodsAs a counterpoint to the more principled approaches that we will begin to study in the next chapter and beyond, it is worth mentioning two heuristic methods that are often used in practice.  By "heuristic" it is meant that these methods are often useful to achieve satisfactory performance, but offer no guarantees about optimality or success.  Behavior scriptingIn behavior scripting, the control signal sent to the robot's actuators is a direct function of its input sensor signals, which is programmed as a body of procedural code; e.g., a set of arithmetic operations and if-else statements.  This programmed logic becomes the body of the "planner", which does not actually explicitly plan anything. Behavior scripting has the appeal in that it provides novices with an easy pathway to start experimenting with robot behaviors.  For example, to follow a black line on a sheet of white paper, two brightness sensors can be used, one to the left and the other to the right of the line.  If the left sensor reads "black", turn the wheel more to the left and reduce the forward velocity.  If the right sensor reads "black", turn the wheel more to the right and reduce the forward velocity.  Otherwise, turn the wheel toward center, and increase the forward velocity.  Properly tuned, this behavior will do a reasonable job at following lines that are not too curved at a slow speed.A more complex behavior script can include some notion of memory that is maintained from step to step, such as a list of objects seen or past actions taken.  A common form of memory is used in a _state machine_, where the behavior switches between discrete modes depending on the sensor input or past actions taken.  In a pure state machine, the only thing that needs to be remembered is the index of the current state. State machines will be discussed in more detail in [Chapter 16](RobotControl.ipynbState-machines).With a bit of ingenuity (and a great deal of testing and tuning), engineers can produce behavior scripts that encode sophisticated behavior.  There is some degree of pride in crafting such behaviors, with the intelligence of the engineer manifesting itself in the appearance of intelligent behavior.  However, the challenge of this approach is that as the problem domain becomes more complex, the complexity of the program's logic (and the corresponding difficulty of programming good behavior) tends to grow exponentially.  Generate-and-scoreA straightforward way of performing motion planning is a three step process:1. _Generate_ a set of $N$ paths $p_1,\ldots,p_N$, each of which starts from the robot's current state $x$.2. _Score_ each path using a score function $S(p)$.  Let $p_i$ be the path with minimum score.3. _Execute_ the first part of $p_i$, and then repeat from step 1.Unlike behavior scripting, this _generate-and-score_ approach is indeed a true planner, because the _Generate_ step predicts how the robot will behave and the _Score_ step can perform collision checks and handle other constraints.A huge range of behavior can be obtained by tweaking the path generation and scoring methods.  As an example, a random-path generation approach simply generates $N$ paths at random, and the scoring function will assign infinite cost to any infeasible path. For feasible paths, the score will measure a combination of the length of the path and the distance from the end of the path to the goal. What we can observe with this approach is that its success is highly problem-dependent. If the robot is traveling in a wide open space or around small obstacles, then it works fairly well, although there is a little jittering.This jittering can be reduced by applying some smoothing in postprocessing.  Or, rather than generating random paths, we can use a library of _motion primitives_ that are the same at each iteration, slightly modified to start at the robot's current state.  For example, a mobile robot can move forward, move backward, turn right, turn left, or perform swerving maneuvers left and right.************************![fig:GenerateAndScore](figures/planning/generate-and-score.svg)Figure 4.     A generate-and-score approach for a mobile robot may use a set of local motion primitives, defined in the robot's local frame (egocentric coordinates).  The scoring function will check collisions with each primitive, and will choose to execute the primitive that makes the most progress toward the target (restricted to its collision-free subpath).    ************************This approach can be very computationally light, and even effective at solving some simpler problems.  However, because the generated paths usually do not reach all the way to the goal, this approach assumes that the problem requires minimal amounts of long-range deliberation. For example, consider the problem where the robot must exit a room before making progress to the goal.  Because the robot must sacrifice progress before making progress, it will be incredibly unlikely to find a path out of the first room, either using random paths or motion primitives.  This is known as a _local minimum_, which will be a reoccurring issue throughout robotics.  Representations of pathsA path is a continuous curve $$y : [0,1] \rightarrow \mathcal{C}$$interpolating between initial and goal configurations $q_I = y(0)$ and $q_G = y(1)$.  There are as many ways to represent paths as there are ways to encode functions, but in robotics we typically use a handful of representations.  The most common variants are described here.  Piecewise-linear pathsFor most of the motion planning chapters, we assume that a path is represented as apiecewise-linear curve, or for non-Euclidean configuration spaces, apiecewise-geodesic curve in C-space. This representation is a set of $n$ connected line segments, simply given a list of $n$ configurations $q_0,\ldots,q_n$ satisfying $q_0 = q_I$ and $q_n = q_G$.  These configurations are often known as _milestones_ or _waypoints_.  The interpolating curve is given by$$y(s) = interp(q_i,q_{i+1},sn-i)$$where $i = \lfloor sn\rfloor$ is the zero-based index of the segment addressed by the parameter $s$.This representation is quite versatile, as all paths can be approximated arbitrarily well by a piecewise linear path with sufficiently large $n$.  Also, path evaluation takes $O(1)$ time.  However, derivatives at each milestone are undefined, which means potentially jerky motions and difficulty handling system dynamics.<jupyter_code>#Code for the below figures on piecewise linear curves
%matplotlib inline
%config InlineBackend.figure_format = 'svg'
import math
import matplotlib.pyplot as plt
import numpy as np

def interp_piecewise_linear(qs,s):
    """Interpolates a piecewise linear curve, given by the milestone list qs, at the parameter s in the range [0,1]"""
    if s <= 0: return qs[0]
    if s >= 1: return qs[-1]
    n = len(qs)-1
    i = int(math.floor(s*n))
    u = s*n-i
    return qs[i]+u*(qs[i+1]-qs[i])

#you may play with the milestones here
milestones = np.array([(0,0),(1,1.5),(-2.5,2),(-3.0,1.5)])

#this is the plotting code
plt.figure(num=None, figsize=(9, 3))
plt.subplot(1, 2, 1)
plt.plot(milestones[:,0],milestones[:,1])
plt.xlabel('x')
plt.ylabel('y')
plt.scatter(milestones[:,0],milestones[:,1],c='k',linewidths=None)

plt.subplot(1, 2, 2)
ss = np.linspace(0,1,20)
evaluated = np.array([interp_piecewise_linear(milestones,s) for s in ss])
plt.plot(ss,evaluated[:,0],label="x(s)")
plt.plot(ss,evaluated[:,1],label="y(s)")
plt.xlabel('s')
plt.ylabel('x or y')
plt.legend()
plt.show()<jupyter_output><empty_output><jupyter_text>PolynomialAnother common representation is a degree $d$ polynomial$$ y(s) = poly(\mathbf{c}_0,\ldots,\mathbf{c}_d;s) = \sum_{i=0}^d \mathbf{c}_i s^i $$where $\mathbf{c}_0,\ldots,\mathbf{c}_d \in \mathcal{C}$ are the polynomial coefficients.  This is a _parametric_ representation in which each component of the motion is represented by an independent 1D function.  Hence, in 2D the y component of a curve may not be, in general, a proper function of x, even though both x and y are proper functions of u.<jupyter_code>#Code for the figure below, showing the parametric nature of multivariate polynomial curves
%matplotlib inline
import math
import matplotlib.pyplot as plt
from numpy.polynomial import polynomial

def interp_polynomial(cs,s):
    """Interpolates a piecewise linear curve given by the coefficient list cs"""
    return polynomial.polyval(s,cs)

#you may play with the coefficients here
coeffs = np.array([(0,0),(1,1),(0,3.0),(-6,-4.0)])

ss = np.linspace(0,1,20)
evaluated = np.array([interp_polynomial(coeffs,s) for s in ss])

#this is the plotting code
plt.figure(num=None, figsize=(9, 3))
plt.subplot(1, 2, 1)
plt.plot(evaluated[:,0],evaluated[:,1])
plt.xlabel('x')
plt.ylabel('y')

plt.subplot(1, 2, 2)
plt.plot(ss,evaluated[:,0],label='x(s)')
plt.plot(ss,evaluated[:,1],label='y(s)')
plt.xlabel('s')
plt.ylabel('x or y')
plt.legend()
plt.show()<jupyter_output><empty_output><jupyter_text>A polynomial is infinitely smooth, but in order to represent complex paths, the polynomial must be of high degree.  High degree polynomials often lead to numerical instability, in which small changes to the highest degree terms cause the curve to change wildly.  The coefficients are also difficult to choose, as they can be fairly unintuitive.  SplinesSplines are a cross between polynomials and piecewise linear curves that gives the "best of both worlds" in terms of the flexibility of piecewise linear curves but also the smoothness qualities of polynomials.  They consist of a sequence of piecewise polynomials, all of low degree.  The representation of the path is given by a set of _control points_ which control the shape of the curve. From these points, polynomial coefficients are derived in order to enforce continuity.The general form of a spline with $n$ segments and degree $d$ is given as follows$$y(s) = poly(C_i;sn-i)$$where $i=\lfloor sn \rfloor$ is the segment index and $C_i=(\mathbf{c}_{0,i},...,\mathbf{c}_{d,i})$ is the set of polynomial coefficients for the $i$th segment.  We do not typically store the polynomial coefficients themselves, but instead represent a list of control points $\mathbf{p}_1,\ldots,\mathbf{p}_k$ from which the coefficients of each polynomial are derived.  This is because it is difficult to set up coefficients manually such that the spline is continuous and smooth.  Modifying control points is far easier and more intuitive.  The method for converting control points to coefficients is the _interpolation scheme_.Typical splines are degree 3 or 5, which are known as cubic or quintic splines, respectively.  (Degree 1 splines are actually piecewise linear curves.) These degrees are popular because they allow control point interpolation schemes to dictate the order of the spline's smoothness (derivative continuity). Cubic Hermite splinesThe simplest spline is the cubic Hermite spline, which defines the control points to include milestones and the derivative of the spline at each milestone.  The Hermite interpolation scheme solves for the polynomials to enforce continuity and derivative continuity (C1 continuity).<jupyter_code># Code for the figure below, showing a parametric cubic Hermite curve
%matplotlib inline
from rsbook_code.planning.paths import hermite
import matplotlib.pyplot as plt

p0 = np.array([1,1])
p1 = np.array([3,2])
h0 = np.array([0,-1])
h1 = np.array([-1.5,1.5])
U = np.linspace(0,1,50)
XY = np.array([hermite(p0,p1,h0,h1,u) for u in U]).T
plt.figure(figsize=(12,3))
ax1 = plt.subplot(1,3,1)
ax1.plot(U,XY[0],lw=2,label="Hermite curve")
ax1.scatter([0,1],[p0[0],p1[0]],label="Endpoints")
ax1.plot([0,1],[p0[0],p0[0]+h0[0]],lw=1,label="Tangent at 0")
ax1.plot([0,1],[p1[0]-h1[0],p1[0]],lw=1,label="Tangent at 1")
ax1.set_xlabel("u")
ax1.set_ylabel("x")
ax1.legend()
ax2 = plt.subplot(1,3,2)
ax2.plot(U,XY[1],lw=2,label="Hermite curve")
ax2.scatter([0,1],[p0[1],p1[1]],label="Endpoints")
ax2.plot([0,1],[p0[1],p0[1]+h0[1]],lw=1,label="Tangent at 0")
ax2.plot([0,1],[p1[1]-h1[1],p1[1]],lw=1,label="Tangent at 1")
ax2.set_xlabel("u")
ax2.set_ylabel("y")
ax2.legend()
ax3 = plt.subplot(1,3,3)
ax3.plot(XY[0],XY[1],lw=2,label="Hermite curve")
ax3.scatter(*zip(p0,p1),label="Endpoints")
ax3.plot(*zip(p0,p0+h0),lw=1,label="Tangent at 0")
ax3.plot(*zip(p1,p1-h1),lw=1,label="Tangent at 1")
ax3.set_xlabel("x")
ax3.set_ylabel("y")
ax3.axis("equal")
ax3.legend()
plt.show()
# Jupyter notebook must be run from this directory for this code to work<jupyter_output><empty_output><jupyter_text>Specifically, suppose the segment travels from milestone $\mathbf{m}_0$ to $\mathbf{m}_1$ as the interpolation parameter travels from 0 to 1.  We wish to find a cubic polynomial segment that respects the initial tangent $\mathbf{t}_0$ and the terminal tangent $\mathbf{t}_1$.  In other words, the terminal conditions are$$\begin{split}\mathbf{m}_0 &= poly(C_i;0) \\\mathbf{m}_1 &= poly(C_i;1) \\\mathbf{t}_0 &= \left. \frac{d}{du} poly(C_i;u) \right|_{u=0} \\\mathbf{t}_1 &= \left. \frac{d}{du} poly(C_i;u) \right|_{u=1} \end{split}$$Letting the coefficients $C_i =(\mathbf{a},\mathbf{b},\mathbf{c},\mathbf{t})$, we have $$ poly(C_i;u) = \mathbf{a} + u \mathbf{b} + u^2 \mathbf{c} + u^3 \mathbf{t} $$$$ \frac{d}{du} poly(C_i;u) = \mathbf{b} + 2 u \mathbf{c} + 3 u^2 \mathbf{t} $$so immediately, we see that $\mathbf{a} = \mathbf{m}_0$ and $\mathbf{b} = \mathbf{t}_0$.  We now need to solve for $\mathbf{c}$ and $\mathbf{t}$ such that the linear equations$$\mathbf{m}_1 = \mathbf{m}_0 + \mathbf{t}_0 + \mathbf{c} + \mathbf{t}$$and$$\mathbf{t}_1 = \mathbf{t}_0 + 2 \mathbf{c} + 3 \mathbf{t}$$hold.  To do so we rearrange the above into the matrix equation$$\begin{bmatrix}\mathbf{m}_1 - \mathbf{m}_0 - \mathbf{t}_0 \\\mathbf{t}_1 - \mathbf{t}_{0}\end{bmatrix} =\begin{bmatrix}1 & 1 \\2 & 3\end{bmatrix}\begin{bmatrix} \mathbf{c} \\ \mathbf{t} \end{bmatrix} $$Which has the solution via 2x2 matrix inversion$$\begin{bmatrix} \mathbf{c} \\ \mathbf{t} \end{bmatrix} = \begin{bmatrix}1 & 1 \\2 & 3\end{bmatrix}^{-1}\begin{bmatrix}\mathbf{m}_1 - \mathbf{m}_0 - \mathbf{t}_0 \\\mathbf{t}_1 - \mathbf{t}_{0}\end{bmatrix} = \begin{bmatrix}3 & -1 \\-2 & 1\end{bmatrix}\begin{bmatrix}\mathbf{m}_1 - \mathbf{m}_0 - \mathbf{t}_0 \\\mathbf{t}_1 - \mathbf{t}_{0}\end{bmatrix} . $$(Check that the inverse is correct by inspection.) In other words, $\mathbf{c} = - 3\mathbf{m}_0 + 3 \mathbf{m}_1 - 2\mathbf{t}_0 - \mathbf{t}_1$ and $\mathbf{t} =  2\mathbf{m}_0 -2 \mathbf{m}_1 + \mathbf{t}_0 + \mathbf{t}_1$.Another convenient way of expressing the segment is:$$poly(C_i;u) = (2u^3 - 3u^2 + 1) \mathbf{m}_0 + (-2u^3 + 3u^3) \mathbf{m}_1 + (u^3 - 2u^2 + u)\mathbf{t}_0 + (u^3 - u^2) \mathbf{t}_1.$$ Quintic Hermite splinesThe quintic Hermite spline defines control points to include milestones and their 1st and second derivatives. The interpolation scheme enforce continuity of first and second derivatives (C2 continuity).  Exercises1. There is a crucial difference between planning to exit a maze and actually escaping a maze.  What is it?  (Hint: consider being blindfolded, kidnapped, and then dropped in the middle of a maze.  The blindfold is now removed.  How does this differ from the information available to a planner?)2. Consider a coffee delivery robot, whose goal is to move a coffee cup from a counter to a patron's table. Does this problem require deliberative planning, or could it be solved using a behavior scripting technique?  What assumptions could you build in about your environment to help a behavior script?  If those assumptions were not built in, that is, the robot is expected to act in a bustling coffee shop, how much forward prediction does it need to perform?3. Earlier, we said that if a planner is asymptotically optimal, then "given more time, it will produce progressively better paths." While this statement is true, is the converse also true?  Is there a case in which a planner could produce progressively better paths, but not satisfy the conditions of asymptotically optimality?  Interactive quiz<jupyter_code>#This code must be run from the RoboticSystemsBook folder

# If you are running on Google Colab, uncomment the following code:
#
# %cd ~
# !git clone --depth 1 https://github.com/krishauser/RoboticSystemsBook
# %cd RoboticSystemsBook

from rsbook_code.assessment import quiz
quiz.show("motion_planning")<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.0898816269991185, 'fraction_numerical': 0.011113209923183478, 'mean_word_length': 4.905372745863543, 'pattern_counts': {'":': 0, '<': 18, '<?xml version=': 0, '>': 28, 'https://': 1, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '4', 'dir': 'jupyter-structured-clean-dedup', 'id': '275713', 'license': 'Apache-2.0', 'n_tokens_mistral': 8622, 'n_tokens_neox': 8126, 'n_words': 4235, 'path': 'WhatIsMotionPlanning.ipynb', 'repo_name': 'WangYixuan12/RoboticSystemsBook', 'size': '31764'}