# Planning through Workspace Constraint Satisfaction and Optimization

Weifu Wang and Ping Li

Cognitive Computing Lab
Baidu Research
10900 NE 8th St. Bellevue, WA 98004, USA

{harrison.wfw<EMAIL_ADDRESS>
###### Abstract

In this work, we present a workspace-based planning framework, which though
using redundant workspace key-points to represent robot states, can take
advantage of the interpretable geometric information to derive good quality
collision-free paths for even complex robots. Using workspace geometries, we
first find collision-free piece-wise linear paths for each key point so that
at the endpoints of each segment, the distance constraints are satisfied among
the key points. Using these piece-wise linear paths as initial conditions, we
can perform optimization steps to quickly find paths that satisfy various
constraints and piece together all segments to obtain a valid path. We show
that these adjusted paths are unlikely to create a collision, and the proposed
approach is fast and can produce good quality results.

##  1 Introduction

We present a workspace-based planning framework, taking advantage of the
describable geometries and the simplicity of point-based planning. The
proposed method also integrates the constraint Jacobian method used in many
planning processes, but instead of taking the gradient with respect to time,
we use the gradient to compute the changes the path needs to make to better
satisfy the constraints in the optimized trajectories. The proposed method is
simple to understand and implement, and the result is comparable to the state-
of-the-art sampling-based planning algorithms quality-wise and can help
identify the geometrical and topological differences among paths.

Many modern planning algorithms work in the configuration space. The
introduction of the configuration space revolutionized the understanding of
robot motion and kinematics and helped advance the planning and manipulation
significantly. At the same time, in the configuration space, the trajectories
can be difficult to visualize, the inverse kinematics changes are hard to
derive based on the violations and other constraints, and the configuration
space obstacles are hard to describe. Many researchers have attempted to map
the workspace geometry to the configuration space geometry, but many
unresolved challenges exist.

There has also been an excellent geometrical planning algorithm, including the
famous Piano Mover’s algorithm series (Schwartz and Sharir, 1983b, a, c).
These algorithms are mathematically correct and complete but challenging to
implement. Moreover, the algorithms can become very complex when concave
shapes are involved. The main challenge is that the interactions between
different geometries can be complex. In the proposed work, however, we intend
to use simple geometries like points and spheres to simplify the geometrical
interactions and use geometrical constraints to coordinate the trajectories
among different key-points on the robot.

The proposed framework can be described in simple terms as the following
steps. We conduct workspace decomposition through triangulation and categorize
the free regions of the workspace. We extract adjacency relations among the
free regions and find paths on the adjacency graph on a high level. We find
paths for points starting from various initial positions using straight-line
connections. We ensure these points correspond to different key points on the
robot and do not collide with the obstacles on these straight-line segments.
We then set the distance constraints among the key points on the robot and
coordinate the paths using the initial straight-line segments to satisfy the
distance constraints using constraint Jacobian methods. If there are preferred
configurations for the robot, they can bias the straight-line paths within the
workspace. The paths are then checked for collisions and further sub-divided
if collisions were detected. The key-points include but are not limited to the
endpoints for each rigid link.

Piece-wise, the paths can be close to optimal through the coordination
(constraint satisfaction and optimization) process. The line-segment paths are
optimal in a 2D workspace using a visibility graph on a global level. In a 3D
workspace, such guarantees do not hold, but there are visibility-like
algorithms to find good quality paths efficiently (Durand, 1999; Durand et
al., 2002). We conducted experiments and compared them to sampling-based
motion planning algorithms. Our approach is fast and can find good quality
paths without expensive optimization or consuming much memory. In particular,
we show that our algorithm works well against narrow passages. The proposed
approach can be perceived as a divide and conquer approach in the workspace,
where the intermediate states subdivide the overall problem into locally
solvable geometric problems.

Within the proposed framework, we analyze the nonholonomic constraints. Also,
we demonstrate that the proposed framework is effective when narrow passages
are present. However, some corner cases may require additional processing
within the proposed framework.

##  2 Related Work

Planning is one of the most fundamental problems in robotics. While many
efforts have been spent studying ways to generate good or valid paths, many
challenges still exist in robotics planning. One challenge is that the robots
usually have a high degree of freedom (DOF), leading to high-dimensional
configuration spaces. Another challenge is that the complex robot controls can
hardly be directly integrated into planning.

There have been studies on planning in the workspace, most notably the Piano
Mover’s problem series (Schwartz and Sharir, 1983b, a, c). The authors used
semi-algebraic sets to describe the robots and the obstacles, and the papers
showed a provable correct and complete algorithm for simple geometries.
However, the approach is also very complex, and the algorithms can be hard to
implement. At the same time, as the robots we would like to plan become more
and more complex, the geometric planning methods start to give way to
sampling-based motion planning algorithms.

In the last 1980s, the sampling-based planning algorithms like Rapid-exploring
Random Tree (RRT) (LaValle, 1998; Jr. and LaValle, 2000) and Probabilistic
RoadMap (PRM) (Kavraki et al., 1996) were introduced, and the planning was
conducted in the configuration space (Lozano-Pérez, 1983). The sampling-based
motion planning was simple to implement and was very quick to find solutions
in open spaces, thus was adopted by many researchers and became one of the
most often used planning methods. Since then, there have been many
modifications to PRM and RRT algorithms, including methods trying to push
towards optimal solutions (Karaman and Frazzoli, 2011), and methods to get
around obstacles (Amato et al., 1998), and other methods to bias the sampling
to achieve better sampling efficiency (Gammell et al., 2020).

To further improve the quality of the paths, optimization processes were
introduced to create better paths in the configuration space, such as CHOMP
(Ratliff et al., 2009; Zucker et al., 2013) and STOMP (Kalakrishnan et al.,
2011). These optimizers build upon a known trajectory in configuration space,
optimize towards better dynamic constraints, and can also help avoid
obstacles. Many variations have been introduced to optimize towards different
goals, including more human-like (Dragan et al., 2015; Marinho et al., 2016),
or more energy efficient (Navarra and Cacciagrano, 2018; Luo et al., 2015).

The planning approach has also considered dynamic and control constraints in
various forms. In addition to trajectory optimizers like CHOMP and STOMP,
shortest paths have also been studied for different systems (Dubins, 1957;
Reeds and Shepp, 1990; Balkcom and Mason, 2002; Balkcom et al., 2006, 2018;
Wang and Balkcom, 2018; Wang and Li, 2021, 2022). These shortest paths often
are used as motion primitives, but the existing algorithms to find such paths
can only find solutions when no obstacles exist.

##  3 Workspace Initial Paths

We can have a relatively accurate description of the constraints or obstacles
in the workspace. From the simple polyhedrons to complex triangle meshes, we
can usually derive helpful information from these descriptions, such as
visibility or the existence of narrow passages. In contrast, even simple
geometries can have complex boundaries in configuration space, especially when
the robots are complex. Robots’ structural information and workspace analysis
cannot be reused in different planning scenarios.

When the workspace or the configuration space is relatively empty, sampling-
based motion planning algorithms can quickly find feasible and sometimes even
good quality configuration space trajectories. Currently, sampling-based
motion planning algorithms often are applied in complex environments. Many
modifications and additional steps are added to these simple algorithms to
overcome various challenges in the complex configuration spaces.

The workspace point-based paths serve as a topological guidance within the
planning environment. Connectivity information is the prerequisite for the
existence of paths. Many studies attempted to investigate the topological
information in the configuration space (C-space). The topological properties
in the C-space are challenging to extract and may not be closely related to
workspace information. However, we can show that the Configuration space is
also a fiber bundle, with the base space being the workspace. Therefore, the
workspace topology can be mapped to the C-space topology using the fibers. The
C-space topology can also be represented as a sub-space topology of the
product topology of the workspace topology for different points on the robot.
Knowing the workspace topology (connectivity) can be the basis for deriving
C-space topology and furthering the robot’s path geometry (trajectory).

The following would be the key idea of the proposed framework: segregating the
topology planning from the geometric planning. We know that a valid trajectory
must build on a connected topological path, which depends on a connected
workspace path for points on the robot. From the topological path, i.e., the
workspace point-based path, we can extrude the geometric paths and coordinate
(constrained optimization) the paths for key points to satisfy the geometric
relations on the robot. Eventually, a valid full (kinematic) trajectory is
found for the robot. We show the basic outline of the framework and
demonstrate that the framework has advantages over sampling-based motion
planning in some scenarios.

A point or a sphere’s path is easy to find in the workspace even when the
obstacles are complex. Let us select $k$ key-points on the robot with
appropriate radii $r_{\cdot}$ to approximate the geometric properties of the
robot at these points. By key-points, we select the endpoints for each link,
but other points can be included for other purposes. Distance constraints
exist among these key points and are fixed with respect to the robot. Suppose
the path for each key point has $r_{\cdot}$ clearance to the obstacles, then
if we adjust the paths to satisfy the distance constraints among the key
points at each time step, the resulting path is most likely to be valid for
the entire robot. The main assumption is that following line-segment paths,
when the two ends of a link have no collision, the entire link is most likely
to be collision-free. We can then approximate the entire robot as a collection
of links with different-sized endpoints.

######  Lemma 1.

In a 2D workspace, given a rigid link of length $l$ with end-points $p_{a}$
and $p_{b}$, and paths for $p_{a}$ and $p_{b}$ without collision and the
distance between $p_{a}$ and $p_{b}$ remains $l$ along the path. Then, the
area swept by the link is bounded by the paths followed by $p_{a}$ and
$p_{b}$. If the paths for $p_{a}$ and $p_{b}$ are simple without loops, the
link will not collide with the obstacle along the path.

###### Proof.

Along the straight lines, the area swept by a link is bounded by the paths of
the endpoints, i.e., the boundary of the swept area are the paths for the
endpoints, plus the initial and final link position. Since the endpoint has no
collision, the link has no collision. ∎

In 2D, it is easy to get the shortest point-based paths using visibility graph
(Ghosh, 1997), and the path can easily be adjusted for circles with radii $r$
without collision. Moreover, this method makes it easy to find paths among
narrow passages. Using segment paths for each key point, we then adjust the
paths to satisfy the distance constraints so that the resulting paths can
directly be applied to robots.

The same method has two main challenges in 3D. First, in 3D, it is not
guaranteed that the visibility graph can produce the shortest paths. Second,
the swept surface is no longer guaranteed to be collision-free even when the
paths for the endpoints are collision-free. Compared to the 2D visibility
graphs, the number of 3D shortest path candidates can grow exponentially.
Direct search for the combination of the vertices and edges of the obstacles
can be expensive. Methods like 3D visibility graphs can help to reduce the
computation complexity but can still be expensive.

Using triangulation, we introduce a workspace decomposition method that will
categorize free regions of the workspace based on the number of adjacent
obstacles. We only briefly describe the how one may get such a workspace
decomposition and path, omitting some of the technical details and proof of
correctness. More details are provided in Wang and Li (2022). Each free-region
is approximately-convex. If the collision-free paths are followed by the
endpoints of a link belonging to the same free region, the path followed by
the link is most likely to be collision-free. Some cases break the above
assumption, but we will not focus on these cases in this work. We then extract
the shortest paths between adjacent free regions to find the globally shortest
path.

The categorization process can be described as follows. First, we construct
triangulation among the free regions using the vertices of the obstacles.
Triangulation (triangle or tetrahedron) can contain vertices from a single
obstacle (and the boundary of the space), two, or three disjoint obstacles. If
the triangulation contains vertices from no more than two obstacles, label
them as adjacent to those obstacles. One free region can exist adjacent to a
single or two obstacles, but concave regions on a single obstacle can have a
triangulation component with all vertices from the same object. Therefore, the
triangulation process starts after the convex hull of each obstacle is
constructed, and intersecting convex hulls are merged after identifying common
regions. If the triangulation component contains vertices from three or more
disjoint obstacles, the corresponding free region may be adjacent to more than
three obstacles. Therefore, we need to merge these free regions if they share
common boundaries. We label the free region as a common region among these
obstacles, whose vertices are part of the merged triangulation components.

Based on the categorization, we can assign a label to each free region, coded
by the number of adjacent obstacles and the corresponding obstacle labels,
using Godel numbering. We can also identify adjacent free regions with labels
and construct an adjacency graph $G_{\mathcal{A}}$ among the free regions. If
an obstacle has a sharp edge or pointy spikes, it is usually marked as the
boundaries of different free regions unless a pillar-like geometry is fully
contained in the area adjacent to only two obstacles. In such cases, we can
treat such geometries as pillars and further partition the free region to
avoid collision with the sweeping surface of the link endpoints. We will omit
these cases in this work, as the work’s main contribution is the workspace-
based planning framework. The adjacency graph shows the connectivity relation
of the workspace, and the paths on the graph correspond to workspace
topological paths. These paths and the robot topology (structural information)
form the C-space topology.

We then plan piece-wise linear paths for each key point within each free
region. We will ensure that the start and the end of these piece-wise linear
paths satisfy the distance constraints among the key points. Using these
piece-wise linear paths, we then adjust the paths of the endpoints to satisfy
the distance constraints.

There are flaws to the proposed approach. In particular, the distance traveled
by the key points does not always correspond to the same amount of changes in
configuration space. For example, consider a humanoid robot, a slight rotation
in the hip joint may lead to the hands moving by a large amount. Though the
change in configuration space is small, the distance traveled by the hand is
significant. On the other hand, the hand’s movement also symbolizes the amount
of energy the hip joint needs to spend to move the hand, as the work done
(distance traveled) in the workspace is one of the most straightforward ways
of measuring the energy.

We can use the geometrical information to adjust the robot state to avoid
collisions in the workspace. The same type of derivation is complex in
configuration space. Again let us consider a humanoid robot, and we have the
robot in collision with the obstacle at the right arm region. If we treat the
robot as a collection of links, we can find the nearest key point to the
collision and move the key point away from the collision point, especially if
we have information such as penetration distance. Then, we can quickly adjust
the robot state by moving the relevant part of the robot and leaving most of
the robot in place.

In contrast, in the configuration space, the inverse kinematic procedure must
be performed to find the necessary movement for the robot. However, other
collisions may be introduced if the movement is performed at an inappropriate
joint. Building upon the workspace partition, we can find workspace paths as
follows:

  1. 1.

Construct triangulation for the workspace and find free regions and their
labels.

  2. 2.

Construct an adjacency graph for the free regions.

  3. 3.

Select a path on the adjacency graph connecting start and goal with a low
estimated cost.

  4. 4.

Test if the sequence of free regions can allow a path for the robot; if not,
return to step three.

  5. 5.

Find collision-free visibility path for the key-point with most neighbors on
the robot.

  6. 6.

Find simple straight-line paths for other key points.

In this paper, our primary focus is to show that the workspace optimization
and coordination among multiple points are feasible, and the details for
obtaining the piece-wise linear paths are only briefly described above. Many
existing processes can be employed to help find the paths, e.g., 3D visibility
algorithms (Durand, 1999; Durand et al., 2002).

When planning the piece-wise linear paths for the endpoints, we adapt a
visibility principle: choose the straight-line connections with minimum
crossings from the start to the targeted region among key points. In case such
simple paths collides with the obstacles, we adapts the following strategy to
find a valid path. First, we need to identify a hierarchical order for the key
points, with one being the root or the primary key point. We refer such
hierarchical order as the topology of the robot. Then, we will ensure the
primary key point is collision free following a simple strategy, such as
visibility. Then, we follow a Breadth First Search type of approach to rotate
all other key points to stay clear of the obstacle, until all key points are
collision free. This outcome of the proposed approach is similar to that of
the obstacle-based PRM (Amato et al., 1998), but the search space is the
workspace. The desired outcomes of the two approaches are similar: find a
valid configuration near the obstacles.

input : $P$: the current robot state; $G$: visibility graph or free region
adjacency graph

output : A new state

Pick base point $p_{o}$;

Find next point $q$ on the shortest point path for $p_{o}$;

$\hat{p}_{o}\leftarrow q$;

Adjust $\hat{p}_{o}$ to be collision free;

$Q\leftarrow p_{o}$ ($Q$: Queue);

$V\leftarrow\emptyset$ while  _Exist point not planned_ do

$p_{c}\leftarrow$ pop $Q$;

$V\leftarrow p_{c}$;

$N\leftarrow$ neighbors of $p_{c}$ on robot;

Push all element in $N$ into $Q$;

for  _$p\in N$_ do

$\vec{d}\leftarrow$ direction from $P(p)$ to $p_{c}$;

$l\leftarrow$ distance between $p$ and $p_{c}$;

$\hat{p}\leftarrow p_{c}+l\frac{\vec{d}}{\|\vec{d}\|}$;

Adjust $\hat{p}$ to avoid collision;

return $\hat{p_{\cdot}}$;

Algorithm 1 FindNextState ($P$)

Once how the robot may travel from the start to the goal is found, i.e., a
topological path (connectivity) is found, we can use Algorithm 1 to find the
intermediate states. The main goal is to admit collision-free piece-wise
linear paths between adjacent states. Moreover, this algorithm attempts to
avoid self-crossing through the visibility principle, reducing the need to add
more intermediate states during planning.

We assume the free region can allow the robot to pass in the above algorithm.
The returned configuration can still have a collision with the obstacles. We
can use penetration depth to find the direction for the adjustments. If a
collision cannot be avoided, we will choose another topological path for
planning.

##  4 Workspace Path Optimization

Once the paths for the endpoints are given, we need to adjust them to ensure
they can be followed by the robot while satisfying the rigid link constraints,
i.e., distance constraint. We first show the process for optimizing with
respect to generic distance constraints. We then discuss other constraints and
show some simple analyses and results. Note that the paths computed using the
proposed algorithm are kinematic. The main advantage of the proposed approach
is that the previously found paths provide good initial conditions for the
optimization, so the process can be fast and is guaranteed to converge.
Further optimizations can be introduced to improve the quality of the
resulting paths.

###  4.1 Geometric constraints

Distance constraint is one of the fundamental geometrical constraints on
robots. When using configurations, the distance constraints are converted to a
variable of joint angles and link lengths. One of the main motivations for
using configurations is to reduce redundancy in the representation of the
robot state. However, the configuration changes do not map geometric
properties well in the workspace. For example, finding a good nearby
configuration to avoid the current collision is difficult. In the workspace,
however, finding a collision-free movement direction for each key point is
much easier and interpolating the motion of other points to satisfy distance
constraints while remaining collision-free.

In general, let us represent the distance constraint as follows, given two
endpoints $p_{a}$ and $p_{b}$ of a link with length $l$:

$\displaystyle C:\|p_{a}-p_{b}\|_{2}=l\ .$ (1)

Constraint Jacobian method is often used when planning for multiple points
with different constraints. For example, in Littlefield et al. (2019), the
authors used constraint Jacobian $\frac{\partial C}{\partial
t}=\mathbf{J}\mathbb{v}$ to compute the adjustments needed for the tensegrity
robot endpoints to satisfy distance constraints and compute appropriate cable
lengths. In most applications of constraint Jacobian, the $\mathbf{J}$ is used
to help compute the velocity adjustment to the next time-step to satisfy
constraint better. In the proposed approach, we will have a complete path for
each point as the initial condition, and we compute the Jacobian for the
constraints to find the adjustments needed for each point to satisfy the
constraints and start the next iteration of optimization. So, instead of
taking derivatives with respect to time, we take derivatives with respect to
the coordinates of the key-points,

$\displaystyle\frac{\partial C}{\partial
p^{i}_{\cdot}}=\mathbf{J}\Delta{p_{\cdot}}$ (2)
$\displaystyle\|p^{i}_{a}-p^{i}_{b}\|=d$ (3) $\displaystyle
p^{i+1}_{\cdot}=p^{i}_{\cdot}+\frac{l-d}{2}\Delta{p_{\cdot}}$ (4)

Given trajectories for $p_{a}$ and $p_{b}$, endpoints of a rigid link of
length $l$. Using constraints in Eq. (1), and follow update rule in Eq. (2)
and Eq. (4), the resulting paths will satisfy the distance constraints. When
no objectives are present, the update reduces violation. Since the updates
between different points are independent, no conflict will occur. The
constraints can be satisfied.

The satisfied distance constraint does not mean the overall trajectory is of
good quality. Even though the initial conditions are the lower bounds for the
path lengths in the proposed approach, the updates may lead to longer paths
than necessary to satisfy the distance constraints. Therefore, in order to get
good quality paths, we further introduce an objective function:

$\displaystyle\min\sum_{i=1}^{N-1}\|p_{a}(i+1)-p_{a}(i)\|$ (5)

We can further transform the above minimization objective into a different
form:

$\displaystyle\min\|p_{a}(i+1)-p_{a}(i)\|+\|p_{a}(i+2)-p_{a}(i+1)\|-\|p_{a}(i+2)-p_{a}(i)\|$
(6)

We can then introduce a small step-size $\alpha$ to reduce the total path
length. When no obstacle exists, and no velocity limit exists for either
endpoint, the shortest path can be a straight line, while the other follows a
trajectory that satisfies the distance constraint. The above constraint is
only for a single key point. We can add more key points to the objective
function and achieve the smallest movement for all key points. Further, if
there exists velocity constraint, we can have:

$\displaystyle\dot{p_{a}(i)}=p_{a}(i+1)-p_{a}(i)$ $\displaystyle
C:\|\dot{p_{a}(i)}\|\leq v_{a}$ (7)

Eq. (7) can be viewed as another distance constraint, and we can update the
point positions using the same derivations as Eq. (2) and Eq. (4).

Due to the distance constraints, the problem is not convex. Intuitively, one
can discover that the gradient direction for satisfying the distance
constraint and the minimization for the path lengths are almost perpendicular,
satisfying the necessary condition for the convergence of the optimization
process. Current experimental results show the quick convergence for the
proposed process. In Figure 1, we show a $4$R and a $6R$ planar arm and the
optimized shortest paths from straight-line initial paths using less than $10$
iterations.

Figure 1: The paths returned by the proposed approach within less than $10$
iterations return smooth paths with small number of interpolated intermediate
points.

Similar to CHOMP (Ratliff et al., 2009), we can also integrate dynamic
constraints into the optimization and the collision avoidance constraint. It
is easy to integrate collision avoidance constraints, especially for
endpoints. In 2D, collision avoidance constraint is not necessary within a
link. In 3D, collision avoidance for endpoints does not guarantee the free of
collision along with the link. So, we do not consider collision avoidance
constraints in this work.

(a) Linear interpolation may create self intersection.

(b) Adding intermediate configurations avoids self-crossing.

Figure 2: One may need to add intermediate states to avoid self-crossing.

As with smoothness constraint, we can similarly add penalties to sudden large
changes in acceleration, the second derivatives along the trajectory. However,
the existing minimization and velocity bound constraint can produce smooth
trajectories from the numerical experiments. For example, as shown in Figure
1, the trajectories for different endpoints (shown in different colors) are
all smooth curves, even though all paths started as straight line segments.

Another constraint we need to consider is the no self-collision on the robot.
Some intermediate configurations may contain self-intersection when we use the
straight-line paths as initial solutions. As shown in Figure 2(a), the first
and last links cross near the parallel configuration, and both the links would
cross the second link along the path. In this case, we need to add
intermediate states to avoid self-collision. For example, between the two
folded states shown in Figure 2(a), we can easily find a path satisfying all
constraints, as shown in Figure 2(b). The self-intersection can be found
through collision detection. Once the collision is identified, a new
intermediate configuration will be added. We will follow the unfolding of the
carpenter’s rule procedure (Abbott et al., 2009) to find what is similar to an
unfolded state for the colliding links. This problem, similar to singularity,
is also present in other planning approaches.

In the experiments conducted, while minimizing the length of the paths, when
only the distance constraint is present, the process always converges, and the
Hessian matrix of the Lagrangian are always positive semi-definite. In this
case, both the objective functions and the constraints are of the form
$\|p_{a}-p_{b}\|_{2}=\sqrt{(x_{a}-x_{b})^{2}+(y_{a}-y_{b})^{2}}$, therefore
the Hessian contains elements of the form

$\displaystyle\frac{g(x_{a},y_{a},x_{b},y_{b})}{((x_{a}-x_{b})^{2}+(y_{a}-y_{b})^{2})^{\frac{3}{2}}}$
(8)

where $g(x_{1},y_{1},x_{2},y_{2})$ can be any of the following

$\displaystyle\pm(y_{a}-y_{b})^{2}$ (9) $\displaystyle\pm(x_{a}-x_{b})^{2}$
(10) $\displaystyle\pm(x_{a}-x_{b})(y_{a}-y_{b})$ (11)

The diagonal elements are all positive square terms. If we consider only two
points, where the objective is the minimization of the length for $p_{a}$ and
$p_{b}$, and the constraint is the distance between $p_{a}$ and $p_{b}$, we
can have the Hessian of the following form

$\displaystyle\Delta_{xx}L=\begin{bmatrix}A|B\\\ \hline\cr B|C\end{bmatrix}$
(12)

Both $A$ and $C$ have positive diagonal elements, and $B$ has negative
diagonal elements. When more key points are being considered, the Hessian can
be divided into more blocks. Each block either describes the second-order
derivatives between two key points similar to $B$ or describes the second-
order derivatives for the same key point at a different time-step similar to
$A$. We want to prove the convergence of the optimization process in future
work.

###  4.2 Nonholonomic constraint

Another common constraint type in motion planning is velocity constraints,
such as no side-way velocity constraint for systems like Dubins and Reeds-
Shepp vehicles. There do not exist good steering functions for these
nonholonomic systems, and inverse kinematics is hard to find using a simple
procedure. Methods like Rapid exploring Random Tree (RRT) (Jr. and LaValle,
2000) are commonly used to find paths for such systems. The quality of the
paths is only guaranteed to approach optimal asymptotically, and the
information is hard to reuse for planning between different start and goals.
Existing solutions to optimal paths for such nonholonomic systems are only
guaranteed to be resolution complete, and only numerical solutions exist
except for some highly symmetric systems (Dubins, 1957; Reeds and Shepp, 1990;
Balkcom and Mason, 2002; Balkcom et al., 2006; Wang and Balkcom, 2012a, b).

In general, the nonholonomic constraints can be written as $W\cdot\dot{q}=c$,
where $q$ is the state of the robot, and the $\dot{q}$ is the velocity of the
robot. Constant $c$ is known to the system, and $W$ is a matrix that depends
on the current state of the robot. For example, for Dubins car or Reeds-Shepp
car, the constraint can be written as:

$\displaystyle\begin{bmatrix}-\sin(\theta)\\\ \cos(\theta)\\\
1\end{bmatrix}\cdot\begin{pmatrix}\dot{x}\\\ \dot{y}\\\
\dot{\theta}\end{pmatrix}=0$ (13)

when we represent the robot using the $q=(x,y,\theta)$ configuration. In the
workspace, we will need to convert the state representation from
$q^{c}=(x,y,\theta)$ to $q^{w}=(x_{1},y_{1},x_{2},y_{2})$ for 2D rigid body,
and change the corresponding parameter for $W$. Using $p_{1}=(x_{1},y_{1})$
and $p_{2}=(x_{2},y_{2})$, we can write the constraint to be the direction of
$\dot{p_{1}}$ must be parallel to $\vec{p_{1}p_{2}}$ at next time-step.

When representing the velocity constraint, we need to normalize both vector to
make sure the gradient is meaningful. Without loss of generality, let us
assume $\|\dot{p_{1}}\|_{2}=\|p_{2}-p_{1}\|_{2}=1$. We can have the following:

$\displaystyle\begin{bmatrix}x_{2}+\dot{x_{2}}\Delta t-x_{1}-\dot{x_{1}}\Delta
t\\\ y_{2}+\dot{y_{2}}\Delta t-y_{1}-\dot{y_{1}}\Delta
t\end{bmatrix}\cdot\begin{pmatrix}-\dot{y_{1}}\\\ \dot{x_{1}}\end{pmatrix}=0$
(14) $\displaystyle\Rightarrow\begin{bmatrix}\frac{\partial
C}{\partial\dot{x_{1}}}\\\ \frac{\partial C}{\partial\dot{y_{1}}}\\\
\frac{\partial C}{\partial\dot{x_{2}}}\\\ \frac{\partial
C}{\partial\dot{y_{2}}}\end{bmatrix}=\begin{bmatrix}y_{2}-y_{1}+\dot{y_{2}}\Delta
t\\\ x_{1}-x_{2}+\dot{x_{2}}\Delta t\\\ -\dot{y_{1}}\Delta t\\\
\dot{x_{1}}\Delta t\end{bmatrix}=\mathbf{J}\Delta\dot{q}$ (15)

Figure 3: Updated velocities for different initial conditions. Only when the
initial velocity is very close to the direction of the link vector (right),
the gradient shown in Eq. (15) will push the robot to follow a rotational
control (left).

Only when the current $\dot{p_{1}}$ is translation along the direction of
$\vec{x_{1}x_{2}}$, the gradient of $\frac{\partial C}{\partial\dot{y_{1}}}$
will be along the direction of $\vec{x_{1}x_{2}}$. Otherwise, the gradient
will push the robot more and more toward a rotational control, as shown in
Figure 3. Following the derivation, we can see that the translation is not
stable for Dubins-like vehicles. Even though the straight line segments are
good initial conditions for path lengths, it is not good for control
optimization, which can easily be shown in the velocity space (Figure 4).
Directly using the proposed process will push the controls towards rotation
and eventually near a turn-drive-turn-like trajectory, oscillating between
optimization iterations.

Figure 4: The velocity for two key-points after satisfying distance
constraints (left), and the same velocities converted to $(v,\omega)$ space
(right).

Converting to the $(v,\omega)$ velocity space, we will see the following
velocity scatter shown on the right of Figure 4. The optimal control theory
proved that the velocity would maximize towards the corners in the control
space to minimize the trajectory length. In this case, the corners are $(1,1)$
and $(1,-1)$ after scaling. The straight-line path provides a good initial
condition for the geometric optimization, but the resulting trajectory is bad
for optimizing against the nonholonomic constraints.

The nonholonomic constraint of $W\cdot\dot{q}=c$ is known to be challenging to
integrate. In the analysis above, we can see that the derivative of this
constraint is also unstable, especially near translation for the Dubins-like
system. The main challenge to integrating the nonholonomic constraint is
whether the resulting trajectory can reach the goal. Therefore, It is very
likely that the velocity update needs to consider the constraints coming from
reaching the goal. Future work’s main focus is finding a stable optimization
process to find paths that satisfy the nonholonomic constraints.

##  5 Experiments

We show some experiments for the complete proposed process, from partitioning
the workspace to finding piece-wise linear paths for multiple key points, and
then optimize to approach optimal paths for the given robot from start to
goal. We further compare the proposed approach with sampling-based motion
planning algorithms conducted in the configuration space, such as PRM and RRT.
The proposed approach aims to find collision-free good, quality kinematic
paths for the given robot in the environment from scratch, rather than
optimizing paths after an initial valid path is found. In this section, we
consider only the distance constraints on the robot.

Let us consider the 2D environments with multiple random polygonal obstacles
and some with narrow passages. We consider planning for a four-link arm with a
moving base. All links have the same width. In Figure 5, we show an example of
the planning. The environment has only a few obstacles, but the clearance
between obstacles is small. The goal is at the bottom right corner.

Figure 5: A 2D environment with a 4R arm, starting from the configuration
among the obstacles to the goal on the lower right corner. The path and
intermediate configuration found using the proposed method are shown on the
top. The 4R arm needs to reverse to reach the goal configuration near the
goal. An illustration of the optimized reversing trajectory is shown at the
bottom.

Using the proposed approach, we first find the visibility path in the
environment and then use Algorithm 1 to find the intermediate states. We then
use the optimization process to interpolate the path between each adjacent
configuration. The direct line paths have a collision with the obstacle
between the second and the third intermediate state, so another intermediate
state is introduced to avoid the collision. The method is to find the
visibility path between two endpoints and add the intermediate collision-free
state as a transition. The proposed approach has several intermediate states
compared to sampling-based motion planning, but the number of intermediate
states is much smaller than the number of samples used in a PRM or RRT.

Once the feasible collision-free intermediate states are found, we use the
process in Section 4.1 to find a feasible path between adjacent states. In
Figure 5, using Algorithm 1, we find the last intermediate state with the
links almost parallel to the goal state, but the entire orientation for the
serial arm is inverted. Therefore, a rotation in place is needed to adjust the
orientation. At the bottom of Figure 5, we show a trajectory for the key
points to achieve a similar change in orientation.

We will need to overcome the relatively long and narrow passage using
sampling-based motion planning. Since the free region is small, the collision-
free samples are hard to find in the given environment, further improving the
cost of sampling-based planning. In the environment shown in Figure 5, no
valid path is found after placing $4000$ samples. The runtime for placing
these samples takes many minutes, while the method proposed in this work took
a fraction of a second to find a valid path.

Of course, in relatively open space, the sampling-based motion planner, such
as PRM or PRM∗, can find a solution relatively fast, while the quality of the
path takes a while to approach the quality found by the proposed approach.
When a path is found using PRM∗, the path quality is very close to that found
by our proposed method. However, in narrow passage environments, as shown in
Figure 5, the time cost of PRM∗ is much greater than the proposed method for
the serial arms.

The proposed approach works well when the obstacles are polygons or
polyhedrons with few vertices. The proposed approach may become slower when
the environments have many obstacles made with a massive number of triangle
meshes. Instead of directly checking for geometric collisions, the proposed
approach needs to analyze the vertices to find piece-wise linear paths.
Therefore, in a more complex environment, we may use a convex hull of the
obstacles to find how to connect the start and goal and use the geometry of
each obstacle to check for collision later.

In the meanwhile, the proposed method can also work well when part of the
robot needs to remain in place while exploring the feasible state of other
parts, such as humanoid walking. Though not explored experimentally in this
paper, we will try to integrate the nonholonomic and other physical
constraints, such as gravity and inertia, into the model.

##  6 Conclusion

We propose a planning framework that mainly plans in the workspace. Rather
than considering the geometry of the entire robot, we use the geometries of
the obstacles to partition the workspace and generate good piece-wise linear
paths for various key-points on the robot. Then, optimizations are conducted
to coordinate the paths to yield valid paths for the robot. The proposed
process plans for points in the workspace and then applies simple optimization
steps to converge quickly on good-quality paths. We compared the proposed
approach with sampling-based algorithms and found that the proposed approach
can be faster than the sampling-based motion planning to generate similar
quality paths and have more advantages in environments with narrow passages.
The proposed approach can be perceived as a divide and conquer approach in the
workspace, where the intermediate states subdivide the overall problem into
locally solvable geometric problems.

## References

  * Abbott et al. [2009] Timothy G Abbott, Erik D Demaine, and Blaise Gassend.  A generalized carpenter’s rule theorem for self-touching linkages.  _arXiv preprint arXiv:0901.1322_ , 2009.
  * Amato et al. [1998] Nancy M Amato, O Burchan Bayazit, Lucia K Dale, Christopher Jones, and Daniel Vallejo.  OBPRM: An obstacle-based PRM for 3D workspaces.  In _Proceedings of the Third Workshop on the Algorithmic Foundations of Robotics (WAFR)_ , pages 155–168, Houston, TX, 1998.
  * Balkcom and Mason [2002] Devin J. Balkcom and Matthew T. Mason.  Time optimal trajectories for bounded velocity differential drive vehicles.  _Int. J. Robotics Res._ , 21(3):199–218, 2002\.
  * Balkcom et al. [2006] Devin J. Balkcom, Paritosh A. Kavathekar, and Matthew T. Mason.  Time-optimal trajectories for an omni-directional vehicle.  _Int. J. Robotics Res._ , 25(10):985–999, 2006\.
  * Balkcom et al. [2018] Devin J. Balkcom, Andrei A. Furtuna, and Weifu Wang.  The Dubins car and other arm-like mobile robots.  In _Proceedings of the 2018 IEEE International Conference on Robotics and Automation (ICRA)_ , pages 380–386, Brisbane, Australia, 2018.
  * Dragan et al. [2015] Anca D. Dragan, Katharina Mülling, J. Andrew Bagnell, and Siddhartha S. Srinivasa.  Movement primitives via optimization.  In _Proceedings of the IEEE International Conference on Robotics and Automation (ICRA)_ , pages 2339–2346, Seattle, WA, 2015.
  * Dubins [1957] Lester E. Dubins.  On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents.  _American Journal of Mathematics_ , 79(3):497–516, 1957.
  * Durand [1999] Fredo Durand.  _3D Visibility: Analytical Study and Applications_.  PhD thesis, Universite Joseph Fourier, Grenoble, France, July 1999.
  * Durand et al. [2002] Frédo Durand, George Drettakis, and Claude Puech.  The 3d visibility complex.  _ACM Trans. Graph._ , 21(2):176–206, 2002.
  * Gammell et al. [2020] Jonathan D. Gammell, Timothy D. Barfoot, and Siddhartha S. Srinivasa.  Batch Informed Trees (BIT*): Informed asymptotically optimal anytime search.  _Int. J. Robotics Res._ , 39(5), 2020.
  * Ghosh [1997] Subir Kumar Ghosh.  On recognizing and characterizing visibility graphs of simple polygons.  _Discret. Comput. Geom._ , 17(2):143–162, 1997\.
  * Jr. and LaValle [2000] James J. Kuffner Jr. and Steven M. LaValle.  RRT-Connect: An Efficient Approach to Single-Query Path Planning.  In _Proceedings of the 2000 IEEE International Conference on Robotics and Automation (ICRA)_ , pages 995–1001, San Francisco, CA, 2000.
  * Kalakrishnan et al. [2011] Mrinal Kalakrishnan, Sachin Chitta, Evangelos A. Theodorou, Peter Pastor, and Stefan Schaal.  STOMP: stochastic trajectory optimization for motion planning.  In _Proceedings of the IEEE International Conference on Robotics and Automation (ICRA)_ , pages 4569–4574, Shanghai, China, 2011.
  * Karaman and Frazzoli [2011] Sertac Karaman and Emilio Frazzoli.  Sampling-based algorithms for optimal motion planning.  _Int. J. Robotics Res._ , 30(7):846–894, 2011\.
  * Kavraki et al. [1996] Lydia E. Kavraki, Petr Svestka, Jean-Claude Latombe, and Mark H. Overmars.  Probabilistic roadmaps for path planning in high-dimensional configuration spaces.  _IEEE Trans. Robotics Autom._ , 12(4):566–580, 1996.
  * LaValle [1998] Steven M. LaValle.  Rapidly-exploring random trees : a new tool for path planning.  _The annual research report_ , 1998.
  * Littlefield et al. [2019] Zakary Littlefield, David Allen Surovik, Massimo Vespignani, Jonathan Bruce, Weifu Wang, and Kostas E. Bekris.  Kinodynamic planning for spherical tensegrity locomotion with effective gait primitives.  _Int. J. Robotics Res._ , 38(12-13), 2019.
  * Lozano-Pérez [1983] Tomás Lozano-Pérez.  Spatial planning: A configuration space approach.  _IEEE Trans. Computers_ , 32(2):108–120, 1983\.
  * Luo et al. [2015] Lu-Ping Luo, Chao Yuan, Rui-Jun Yan, Quan Yuan, Jing Wu, Kyoo-Sik Shin, and Chang-Soo Han.  Trajectory planning for energy minimization of industry robotic manipulators using the lagrange interpolation method.  _International Journal of Precision Engineering and Manufacturing_ , 16(5):911–917, 2015.
  * Marinho et al. [2016] Zita Marinho, Byron Boots, Anca D. Dragan, Arunkumar Byravan, Geoffrey J. Gordon, and Siddhartha S. Srinivasa.  Functional gradient motion planning in reproducing kernel hilbert spaces.  In _Proceedings of the Conference on Robotics: Science and Systems (RSS), XII_ , Ann Arbor, MA, 2016.
  * Navarra and Cacciagrano [2018] Alfredo Navarra and Diletta Cacciagrano.  Energy saving and collision-free motion planning for oblivious robots.  In _Proceedings of the 32nd International Conference on Advanced Information Networking and Applications Workshops (AINA)_ , pages 591–596, Krakow, Poland, 2018.
  * Ratliff et al. [2009] Nathan D. Ratliff, Matthew Zucker, J. Andrew Bagnell, and Siddhartha S. Srinivasa.  CHOMP: gradient optimization techniques for efficient motion planning.  In _Proceedings of the 2009 IEEE International Conference on Robotics and Automation (ICRA)_ , pages 489–494, Kobe, Japan, 2009.
  * Reeds and Shepp [1990] James A. Reeds and Lawrence A. Shepp.  Optimal paths for a car that goes both forwards and backwards.  _Pacific Journal of Mathematics_ , 145(2):367–393, 1990.
  * Schwartz and Sharir [1983a] Jacob T Schwartz and Micha Sharir.  On the “piano movers” problem. II. general techniques for computing topological properties of real algebraic manifolds.  _Advances in applied Mathematics_ , 4(3):298–351, 1983a.
  * Schwartz and Sharir [1983b] Jacob T Schwartz and Micha Sharir.  On the “piano movers’” problem I. the case of a two-dimensional rigid polygonal body moving amidst polygonal barriers.  _Communications on pure and applied mathematics_ , 36(3):345–398, 1983b.
  * Schwartz and Sharir [1983c] Jacob T Schwartz and Micha Sharir.  On the piano movers’ problem: III. coordinating the motion of several independent bodies: The special case of circular bodies moving amidst polygonal barriers.  _The International Journal of Robotics Research_ , 2(3):46–75, 1983c.
  * Wang and Balkcom [2012a] Weifu Wang and Devin J. Balkcom.  Analytical time-optimal trajectories for an omni-directional vehicle.  In _Proceedings of the IEEE International Conference on Robotics and Automation (ICRA)_ , pages 4519–4524, St. Paul, MN, 2012a.
  * Wang and Balkcom [2012b] Weifu Wang and Devin J. Balkcom.  Sampling extremal trajectories for planar rigid bodies.  In _Proceedings of the Tenth Workshop on the Algorithmic Foundations of Robotics (WAFR), X_ , pages 331–347, Cambridge, MA, 2012b.
  * Wang and Balkcom [2018] Weifu Wang and Devin J. Balkcom.  Time-optimal motion of spatial dubins systems.  In _Proceedings of the 13th Workshop on the Algorithmic Foundations of Robotics (WAFR), XIII_ , pages 529–544, Mérida, Mexico, 2018\.
  * Wang and Li [2021] Weifu Wang and Ping Li.  Towards finding the shortest-paths for 3d rigid bodies.  In _Proceedings of the Conference on Robotics: Science and Systems (RSS), XVII_ , Virtual Event, 2021.
  * Wang and Li [2022] Weifu Wang and Ping Li.  Towards constructing finer then homotopy path classes.  _arXiv preprint arXiv:2206.06307_ , 2022.
  * Zucker et al. [2013] Matthew Zucker, Nathan D. Ratliff, Anca D. Dragan, Mihail Pivtoraiko, Matthew Klingensmith, Christopher M. Dellin, J. Andrew Bagnell, and Siddhartha S. Srinivasa.  CHOMP: covariant hamiltonian optimization for motion planning.  _Int. J. Robotics Res._ , 32(9-10):1164–1193, 2013.