11institutetext: Department of Informatics, Bioengineering, Robotics, and
Systems Engineering, University of Genoa, Via All’Opera Pia 13, 16145 Genoa,
Italy.
11email<EMAIL_ADDRESS><EMAIL_ADDRESS>
# Minimum Displacement Motion Planning for Movable Obstacles

Antony Thomas  Fulvio Mastrogiovanni

###### Abstract

This paper presents a minimum displacement motion planning problem wherein
obstacles are displaced by a minimum amount to find a feasible path. We define
a metric for robot-obstacle intersection that measures the extent of the
intersection and use this to penalize robot-obstacle overlaps. Employing the
actual robot dynamics, the planner first finds a path through the obstacles
that minimizes the robot-obstacle intersections. The metric is then used to
iteratively displace the obstacles to achieve a feasible path. Several
examples are provided that successfully demonstrates the proposed problem.

##  1 Introduction

Classical robot motion planning problem finds a collision free path from a
given initial pose to a desired goal pose of a robot amidst fixed obstacles
[11]. This problem is well studied. However, in several situations a robot may
need to move all (if no collision free path exists) or certain obstacles along
its path. Mobile robots in a indoor environment may need to move obstacles to
reach a goal. For example, open closed doors or push chairs around to reach a
location. In the case of dynamic obstacles a robot may need to plan a path
through these obstacles as they might clear a path while moving. Manipulator
robots often rearrange or move obstacles aside to complete a task.

Such problems require the knowledge about the environment and the robot’s
motion. A collision between a robot and an obstacle occur when they intersect
each other. If the robot has to clear its way to find a feasible path then the
obstacle must be displaced such that there is no robot-obstacle intersection.
In this paper, we investigate this problem, that is, finding a feasible path
such that the obstacle displacements are minimized (see Fig. 1). Currently we
do not focus on how the displacement happens and assume that the computed
displacements can be achieved irrespective of the type of robot-obstacle
interaction required to move the obstacles.

We present a new motion planning formulation in which the planner first finds
a path through the obstacles such that the robot-obstacle intersections are
minimized. To this end, the planner minimizes a weighted sum of the robot path
length and a metric that measures the extent of robot-obstacle intersection.
This robot-obstacle intersection metric is then used to determine the minimum
displacements for the obstacles so that the computed path is collision free.
While computing the minimum displacements, we also take into account the fact
that the displaced obstacles must themselves be collision free.

(a)

(b)

(c)

(d)

Figure 1: (a) An L-shaped robot in the IAS domain has to move from left to
right by displacing obstacles. (b) Robot takes the shortest path and the 13
displaced obstacles can be seen in cyan color. (c) Robot finds a path with
smaller displacement, moving 10 obstacles. (d) An example of the planning
phase of our approach where a path though the obstacles is obtained that
minimizes the robot-obstacle overlaps.

##  2 Minimum Displacement Motion Planning

Let $\mathcal{C}$ denote the robot configuration space with
$x^{0},x^{g}\in\mathcal{C}$ denoting the start and goal states. Let
$\mathcal{O}=\\{o^{i}|\ 1\leq i\leq n\\}$ denote the set of movable obstacles
in the environment with an associated displacement set $\mathcal{D}=\\{d^{i}|\
1\leq i\leq n\\}$ that represents the corresponding obstacle displacements. By
abuse of notation we will use $o^{i}$ to denote both the $i$-th obstacle as
well as its state. Note that the displacements $d^{i}$ in general may not
always be translational alone and can include rotations or deformations. Thus
$d^{i}$ belongs to a displacement space of arbitrary dimension. In this paper,
we will consider obstacles that can either perform translation alone or pure
rotation alone about its center. Therefore $d^{i}$ either represents pure
translation or pure rotation. Initially, $d^{i}=0$. We can now define the
minimum displacement motion planning problem.

###### Definition 1

The minimum displacement motion planning problem from $x^{0}$ to $x^{g}$ finds
a path $x\mathrel{\mathop{\ordinarycolon}}[0,1]\rightarrow\mathcal{C}$ with
obstacle displacements $d^{1},\ldots,d^{n}$ minimizing the cost
$C(x,\mathcal{D})$ given by

$C=w^{x}c(x)+w^{d}\sum_{i=1}^{n}c^{i}(d^{i})$ (1)

such that

  1. 1.

$x(0)=x^{0}$, $x(1)=x^{g}$

  2. 2.

$\bigcup_{i\neq j}\left(\ o^{i}(d^{i})\cap o^{j}(d^{j})\
\right)=\\{\emptyset\\}\ \ \forall i,j\in\\{1,\ldots,n\\}$

  3. 3.

$x(t)\cap\bigcup_{i=1}^{n}o^{i}(d^{i})=\\{\emptyset\\}$, $\forall t\in[0,1]$

In (1), $c(x)$ is a function of the path length, $c^{i}(d^{i})$ are positive
displacement costs with $w^{x}$ and $w^{d}$ being the corresponding weights
and $o^{i}(d^{i})$ denotes the new obstacle locations. The first condition
satisfies the endpoint constraints. The second condition ensures that the
displaced obstacles are themselves collision free and the last condition
guarantees the robot to not intersect with the displaced obstacles.

##  3 Algorithm for Minimum Displacement Motion Planning

In this section we present our planner for minimum displacement motion
planning that employs a two stage optimization approach, namely, (a) the
planning phase and (b) the refinement phase.

###  3.1 (a) Planning Phase

Let us consider a robot with a general dynamics model of the form

$x_{k+1}=f(x_{k},u_{k})$ (2)

where $x_{k+1}$, $x_{k}$ are the robot states at time $k+1$, $k$ respectively,
and $u_{k}$ is the applied control at $k$. We define a function
$V(x^{\prime},o^{i})$ to measure the robot-obstacle intersection or overlap,
where $x^{\prime}=g(x)$ denote the relevant robot state (at any time instant)
of interest and $o^{i}$ is the obstacle state of interest. For example, if the
robot and obstacle are spheres of radii $r^{r}$ and $r^{o}$, respectively,
then $V$ is defined as

$V(x^{\prime},o)=\begin{cases}md\ &\text{if}\ \mathinner{\\!\left\lVert
x^{\prime}-o\right\rVert}\leq(r^{r}+r^{o})\\\ 0\
&\text{otherwise}.\end{cases}$ (3)

where

$md=\mathinner{\\!\left\lVert x^{\prime}-o\right\rVert}-(r^{r}+r^{o})$ (4)

with $x^{\prime}$, $o$ denoting the centers of the robot and obstacle spheres,
respectively and $\mathinner{\\!\left\lVert\cdot\right\rVert}$ denoting the
$L_{2}$ norm. We note that if two spheres intersect/collide then
$\mathinner{\\!\left\lVert x^{\prime}-o\right\rVert}\leq(r^{r}+r^{o})$. Thus
$V(x^{\prime},o)$ is essentially the collision condition and measures the
extent of intersection or overlap when the robot and the obstacle collide. It
is easily observed that the magnitude of $md$, that is, $|md|$ is actually the
minimum distance by which the obstacle sphere is to be moved along the line
joining the robot and obstacle sphere centers so as to make this overlap equal
to zero. For non spherical objects, $x^{\prime}$ mostly comprises of the
vertices and the center of the polyhedron and the $V(\cdot)$ is the collision
condition between the two polyhedrons. However, in this paper we use minimum
volume spheres as the underlying bounding volumes to tightly enclose a
polyhedron as they provide an efficient approximation.

At each time instant $k$, the robot plans for $L$ look-ahead steps minimizing
an objective function

$J_{k}=\sum\limits_{l=0}^{L-1}c_{l}(x_{k+l},\mathcal{O})+c_{L}(x_{k+L})$ (5)

with $c_{l}$ being the cost for each look-ahead step and $c_{L}$ the terminal
cost. As a result, we have the following immediate and terminal costs

$\displaystyle c_{l}(x_{k+l},\mathcal{O})=\mathinner{\\!\left\lVert
x_{k+l}\right\rVert}^{2}_{M_{x}}+\sum_{i=1}^{n}\mathinner{\\!\left\lVert
V^{i}\right\rVert}^{2}_{M_{i}}$ (6) $\displaystyle
c_{L}(x_{k+L})=\mathinner{\\!\left\lVert
x_{k+L}-x^{g}\right\rVert}^{2}_{M_{g}}$ (7)

where $\mathinner{\\!\left\lVert x\right\rVert}_{S}=\sqrt{x^{T}Sx}$ is the
Mahalanobis norm, $M_{u},M_{g},M_{i}$ are weight matrices. The optimization
problem can now be formally stated as —s—[1] ∑_l=0^L-1[
$\mathinner{\\!\left\lVert x_{k+l}\right\rVert}$ ^2_M_x + ∑_i=1^n
$\mathinner{\\!\left\lVert V^{i}\right\rVert}$ ^2_M_i ] +
$\mathinner{\\!\left\lVert x_{k+L}-x^{g}\right\rVert}$ ^2_M_g x_0=x^0
x_k+l=f(x_k+l-1,u_k+l-1) x_T=x^g u_k+l∈U where (3.1) constraints the control
actions to lie within the feasible set of control inputs. The term
$\mathinner{\\!\left\lVert V^{i}\right\rVert}^{2}_{M_{i}}=M_{i}(md^{i})^{2}$
penalizes the robot-obstacle intersections and the optimization returns the
robot trajectory $x_{0},...,x_{T}$ with minimum obstacle overlaps $md^{i}$ at
each time instant. As discerned before, the magnitude of $md^{i}$ is actually
the minimum displacement of the obstacle $o^{i}$ required to make the robot-
obstacle overlap equal to zero. However, each obstacle $o^{i}$ is not
overlapped by a single robot state $x_{k}$ but $l$ different discretized
states since the robot trajectory is continuous. Moreover, a robot and the
obstacle may be represented by $m^{1}$ and $m^{2}$ different bounding volume
spheres, respectively. As a result, for each obstacle $o^{i}$, we have
$md^{i^{1}},\ldots,md^{i^{p}}$, $1\leq p\leq l\times m^{1}\times m^{2}$
different overlaps with the robot. The maximum among them is the required
distance for zero overlap. The planning phase is summarized below:

Planning Phase:
1\. Input: $x^{0}$, $x^{g}$, $\mathcal{O}$,$y^{1}=0,\ldots,y^{n}=0.$
2\. Compute $x_{0},...,x_{T}$ such that $J_{k}$ is minimized at each
$k\in[0,T]$.
3\. Simulate $x_{0},...,x_{T}$ to compute $\\{md^{i^{j}}\\},\ 1\leq j\leq p$
for each $o^{i}$.
4\. Compute $y^{i}=\max(\\{md^{i^{j}}\\})$, to obtain the distance for each
obstacle $o^{i}$.

Though we have computed the minimum distances to be moved, we have not yet
ensured the condition 2., 3. in Definition 1, that is, the displaced obstacles
being themselves collision free and the robot not colliding with the displaced
obstacles while executing the computed trajectory. The refinement phase of the
planner attends to these concerns.

###  3.2 (b) Refinement Phase

Displacing the obstacles by $y^{i}$ does not ensure the obstacles being
themselves collision free since it may introduce new overlaps with the
obstacles. Similarly, in some cases moving the obstacle by $y^{i}$ can lead to
new intersections with the remaining robot trajectory. To tackle this, we
iteratively increase the displacement magnitudes.

The refinement phase is essentially a displacement sampling process. We note
here that the $y^{i}$ computed is the minimum distance the obstacle sphere is
to be moved along the line joining the robot and obstacle spheres. We note
here that though a given obstacle may be represented using multiple spheres,
$y^{i}$ computed represents the maximum displacement from among all these
spheres. The obstacle center is thus displaced by $y^{i}$ along the maximum
displacement direction. Each displaced obstacle is checked for collision with
the robot and the other obstacles. If no displaced obstacles collide with the
robot and other obstacles then the obtained $y^{i}$ are the required minimum
displacements. However, most often due to the nature of the environment and
the continuous robot trajectory, some of the displaced obstacles introduce new
overlaps, that is, collide either with the robot or other obstacles or both. A
displacement $y^{i}=y^{i}+\Delta$ is computed and obstacle locations at a
distance equal to the newly computed $y^{i}$ from the initial obstacle
location $o^{i}$ are sampled and checked for collision. The displacements are
incremented by $\Delta$ until no collisions are found. The refinement process
is as follows:

Refinement Phase:
1\. Input: $x_{0},...,x_{T}$, $\mathcal{O}$,$y^{1},\ldots,y^{n}$, $\Delta$,
$d^{1}=0,\ldots,d^{n}=0$.
2\. While true
3\. For each $o^{i}$ such that $md^{i^{j}}\neq 0$
4\. Find $y^{i}$.
5\. Run Sample-Displacement.
6\. For $sample$
7\. If $\bigcup_{i\neq j}\left(\ o^{i}(y^{i})\cap o^{j}(y^{j})\
\right)=\\{\emptyset\\}\ \ \forall i,j\in\\{1,\ldots,n\\}$ and $x_{t}\ \cap\
\bigcup_{i=1}^{n}o^{i}(y^{i})=\\{\emptyset\\}\ \forall t\in\\{1,\ldots,T\\}$.
8\. $d^{i}=y^{i}$.
9\. Else
10\. $y^{i}=y^{i}+\Delta$.

The Sample-Displacement subroutine samples obstacle locations at a distance of
$y^{i}$ from the initial obstacle location $o^{i}$. For a 2D scenario, the
samples lie on a circle with center being the initial obstacle location before
displacement, that is, $o^{i}$, and radius equal to the newly computed
$y^{i}$. In a 3D scenario, the circle is replaced by a sphere and samples are
points on the surface of the sphere. If the obstacle can only be rotated, then
a discrete set of rotations are sampled. The subroutine runs as follows:

Sample-Displacement:
1\. Input: $\mathcal{O}$, $y^{i}$.
2\. For $N=1,2,\ldots$
3\. If translation
4\. Sample $o^{i}(y^{i})$ such that $\mathinner{\\!\left\lVert
o^{i}(y^{i})-o^{i}\right\rVert}=y^{i}$.
5\. If rotation
6\. Sample $o^{i}(y^{i})$, such that $0\leq y^{i}<2\pi$.
7\. Return sample $o^{i}(y^{i})$.

###  3.3 Optimality

The cost given in (1) is a combination of the sum of the costs in the planning
phase and the incremental displacements computed in the refinement phase.
Thus,

$C=\sum_{k=0}^{T}\left(J_{k}+\sum_{i=1}^{n}(d^{i}-y^{i})\right)$ (8)

To prove any sort of optimality of our two stage planner, it should exhibit
the optimal substructure property. A problem has an optimal substructure only
if an optimal solution to the problem can be obtained from optimal solutions
to its subproblems [1]. We assume that we have a mechanism by which we obtain
the optimal values for the weights $M_{u},M_{g},M_{i}$. Let
$M_{u}^{\star},M_{g}^{\star},M_{i}^{\star}$ give the optimal cost
$J^{\star}=\sum_{k=0}^{T}J_{k}$. Let the distances returned by planning phase
be $\\{y^{i^{\star}}\\}$. In the ideal scenario, that is,
$\bigcap_{i=1}^{n}o^{i}(y^{i^{\star}})=\\{\emptyset\\}$ and
$\left(\bigcap_{t=1}^{T}x_{t}\right)\cap\left(\bigcap_{i=1}^{n}o^{i}(y^{i^{\star}})\right)=\\{\emptyset\\}$,
and no refinement phase is required. Thus, $d^{i}=y^{i^{\star}}$. $J^{\star}$
is optimal because if there were another $J^{+}\leq J^{\star}$, then we would
use $J^{+}$ since the optimal weights $M_{u}^{+},M_{g}^{+},M_{i}^{+}$ are
known to us.

If the refinement phase is to be run, that is, $d^{i}\neq y^{i^{\star}}$, then
we find $d^{i}$ using incremental sampling. This $d^{i}$ can be made
asymptotically closer to the actual value by choosing a sufficient increment
$\Delta>0$. Thus, for any $\epsilon>0$ that forms an allowable limit on the
suboptimality, $d^{i}\rightarrow d^{i^{\star}}+\epsilon$ with probability 1.
Note that this is a direct consequence of our Sample-Displacement subroutine.
The optimal cost is thus obtained as
$C^{\star}=J^{\star}+\sum_{i=1}^{n}(d^{i}-y^{i^{\star}})$.

Let us consider any other solution $J^{\prime}$ for the planning phase with
$M^{\prime}_{u},M^{\prime}_{g},M^{\prime}_{i}$ being the weights. Based on our
assumption, any weights other than $M_{u}^{\star},M_{g}^{\star},M_{i}^{\star}$
gives $J^{\prime}>=J^{\star}$ and $\sum y^{i^{\prime}}>=\sum y^{i^{\star}}$.
Now since the refinement phase proceeds in an incremental manner, we have
$\sum(d^{i}-y^{i^{\prime}})>=\sum(d^{i}-y^{i^{\star}})$. Therefore we have,
$C^{\prime}=J^{\prime}+\sum_{i=1}^{n}(d^{i}-y^{i^{\prime}})>=C^{\star}$,
giving the desired result.

##  4 Examples and Applications

The nonlinear model predictive control (NMPC) based optimization in (3.1) is
realized using the solver FORCESPRO [4, 21] which generates optimized NMPC
code. Note that the term $V^{i}$ in the objective function (3.1a) is a
conditional penalty term as defined in (3). Since the objective function has
to be continuously differentiable (smooth), the switch between the penalty
value and zero will have to be performed in a smooth manner. To this end, we
employ a built-in smooth approximation function provided by FORCESPRO that
computes the minimum between two
quantities111https://forces.embotech.com/Documentation/modelling/index.html?“#smooth-
minimum. The optimal weights $M_{x}$, $M_{i}$, $M_{g}$ vary with each problem
and the tuning is currently achieved by trial and error. For all the
experiments, we use a look-ahead $L=21$ and an incremental displacement
$\Delta=0.01$. The offline code generation using FORCESPRO (planning phase)
took around 16 seconds for the IAS domain with 35 obstacles. The performance
is evaluated on an Intel® Core i7-10510U<EMAIL_ADDRESS>with 16GB RAM
under Ubuntu 18.04 LTS.

###  4.1 Theoretical Examples

The L-shaped robot example introduced in Fig. 1 has the following non-linear
dynamics model

$\begin{split}\dot{x}_{x}&=ucos(\theta)-vsin(\theta)\\\
\dot{x}_{y}&=usin(\theta)+vcos(\theta)\\\ \dot{\theta}&=\omega\\\ \end{split}$
(9)

where $x=(x_{x},x_{y},\theta)$ is the robot state (location and orientation)
with $u,v$ being the linear velocities and $w$ being the angular velocity.
Fig. 1b shows the shortest path trajectory with the planning phase
displacement $\sum y^{i}=13.08$. The refinement phase gave the final
displacement of $\sum d^{i}=18.52$ which is observed in Fig. 1b. On the other
hand, Fig. 1c in which the displacements are heavily penalized gave $\sum
y^{i}=7.53$ with a final total displacement of $\sum d^{i}=12.12$. Fig. 1d
shows an instance of the planning phase (line 3) where the computed trajectory
is simulated to obtain the set of $\\{md^{i^{j}}\\}$, that is, the set of
robot-obstacle overlaps.

(a)

(b)

Figure 2: A robot moving from left to right in the IAS domain. (a) A
7-obstacle solution. (b) A 10-obstacle solution with smaller displacement.

Fig. 2 shows two instances of the IAS domain where 7 (Fig. 2a) and 10 (Fig.
2b) obstacles are displaced while the robot reaches its goal. Fig. 2a gave a
planning phase displacement of $\sum y^{i}=1.19$ and refinement phase
displacement of $\sum d^{i}=1.32$. However, Fig. 2b gave $\sum y^{i}=1.01$ and
a final total displacement of $d^{i}=1.05$. This example demonstrates that
minimum displacement of obstacles does not always imply minimum number of
obstacles being displaced. We use the following non-linear dynamics for the
robot

$x_{k+1}=\begin{bmatrix}x_{x}\\\ x_{y}\\\ \theta\\\
\end{bmatrix}_{k+1}=\begin{bmatrix}x_{x}\\\ x_{y}\\\ \theta\\\
\end{bmatrix}_{k}+\begin{bmatrix}Dcos(\theta_{k}+T/2)+Ccos(\theta+(T+\pi)/2)\\\
Dsin(\theta_{k}+T/2)+Csin(\theta+(T+\pi)/2)\\\ T\\\ \end{bmatrix}$ (10)

where $x=(x_{x},x_{y},\theta)$ is the robot state and $D$, $C$, $T$ are the
down-range, cross-range and turn control components, respectively.

Fig. 3 exhibits an example of pure rotation. A point robot has to reach its
goal by displacing two blocks that can only be rotated about their centers.
The point robot follows the dynamics given in (10) and its $y-$axis component
$x_{y}$ is constrained to lie between the magenta lines. Fig. 3b shows the
bounding volume spheres used for both the blocks and Fig. 3c displays the
corresponding solution. The total rotation computed is
$\sum_{i=1}^{2}d^{i}=\sum_{i=1}^{2}y^{i}=1.01$. We now relax the assumption of
bounding volume spheres by using the collision condition between a rectangle
and a point in (3) and compute the solution by penalizing the robot whenever
it is inside the rectangle. The resulting trajectory is given in Fig. 3d with
a total displacement of $0.98$. Thus, our use of bounding volume spheres for
enclosing obstacles is a reasonable assumption.

(a)

(b)

(c)

(d)

Figure 3: (a) A point robot needs to move from left to right by moving the two
blocks which can only be rotated about its centers (blue blobs). (b) Bounding
volume spheres used to enclose the obstacles are shown. (c) Solution obtained
using our approach. (d) Assumption of bounding volume spheres is relaxed and
the solution obtained by minimizing robot-rectangle overlap is shown.

###  4.2 Applications

There exists several applications wherein a robot need to move through a
cluttered space by rearranging or repositioning objects along its way–
displacing chairs or other objects of interest. In Fig. 4 different instances
of a robot navigation through the IAS domain among movable obstacles can be
seen. The trajectory and the obstacle displacements are computed offline as
described in Section 3. As the robot moves along the computed trajectory the
obstacles are moved aside with the help of its arms. In the experiment
reported in Fig. 4, the arm motions to displace the obstacles are teleoperated
by a human.

(a)

(b)

(c)

(d)

(e)

(f)

(g)

(h)

(i)

Figure 4: Different instances of a robot navigating the IAS domain by moving
obstacles to reach its goal.

Fig. 5 shows the sofa domain wherein the robot has to reach its goal by
pushing around the sofas. The resulting 4-sofa solution with minimum
displacement is shown in Fig. 5c.

|
---|---

(a)

(b)

(c)

Figure 5: (a) The sofa domain where the robot has to reach the goal by push
around the sofas. (b) Map of the domain with green denoting the starting
location of the robot and red being the goal location. (c) 4-obstacle solution
with the displaced sofas shown in cyan.

##  5 Related Work

Minimum displacement motion planning problem is not an entirely new class of
problems and bears resemblance several existing works in the literature. If
all the displacements equal to zero then we have an instance of the classical
motion planning or the Piano Mover’s problem [13]. By tuning the weights
$w^{x}$ and $w^{d}$ to heavily penalize longer paths, the problem can be
reduced to finding the shortest path (see Fig. 1). Our work is closely related
to the minimum constraint displacement (MCD) [6] and minimum constraint
removal (MCR) [7] motion planning problems. Both MCD and MCR sample robot
configurations and incrementally grows the roadmap (robot configurations
connected by straight lines). We do not sample robot configurations and plan a
path employing the actual robot dynamics to minimize the robot-obstacle
overlaps. MCD also sample obstacle displacements and the iterations (extension
of roadmap and displacement sampling) continue until the best cost solution is
obtained. MCD does not consider obstacle interactions and the displaced
obstacles are allowed to overlap with each other. MCR finds the path in the
roadmap with the fewest geometric constraints or obstacles that need to be
removed to connect the start state with the goal state. However, MCR does not
direct where the obstacles should be moved. By encoding the obstacles with
non-zero overlap to vanish our approach thus generalizes the MCR problem.

Navigation Among Movable Obstacles (NAMO) [15, 12, 19] is a related class of
problems. NAMO is proved to be NP-hard [20] and most approaches solve a
subclass of problems that selects a set of obstacles to be moved and displaces
them to reconfigure the environment. Another related area is the field of task
and motion planning [8, 14, 2, 5, 17, 18]. Selecting the obstacles to be
displace may be accomplished via task planning and the geometric constraints
may be incorporated into the symbolic planner [14] or both the task and motion
planning could be integrated by means of an efficient mapping between the two
domains [2, 18]. Manipulation among clutter or rearrangement planning in
clutter [16, 3, 10, 9] also bears resemblance to the problem proposed herein.
[3, 9] are often suboptimal in the sense that many objects may be moved than
that is necessary due to their planner heuristic.

##  6 Conclusion

We have presented an approach for minimum displacement motion planning that
minimizes obstacles displacements to find a feasible path. The proposed
approach is a two stage process where we first find a path with minimum robot-
obstacle overlaps. A metric is used to measure the extant of overlap and this
in turn is used to incrementally displace the obstacles to achieve the
required feasible path. By appropriately tuning the weights $M_{x}$, $M_{i}$,
$M_{g}$, the planning phase can be made to return the optimal solution.
However, the refinement phase may not always return the optimal solution. For
example an ideal displacement of an obstacle could be a combination of
translation and rotation. However, currently we assume that the obstacle can
either only translate or only rotate and therefore may render the refinement
phase suboptimal. We plan to relax this assumption in future work. We also
plan to generalize this work by doing away with bounding volume spheres to use
the actual polyhedral geometry while computing the robot-obstacle
interactions.

## References

  * [1] Cormen, T.H., Leiserson, C.E., Rivest, R.L., Stein, C.: Introduction to algorithms. MIT press (2009)
  * [2] Dantam, N.T., Kingston, Z.K., Chaudhuri, S., Kavraki, L.E.: Incremental task and motion planning: A constraint-based approach. In: Robotics: Science and Systems (2016)
  * [3] Dogar, M., Srinivasa, S.: A framework for push-grasping in clutter. In: Robotics: Science and systems VII (2011)
  * [4] Domahidi, A., Jerez, J.: Forces professional. Embotech AG, url=https://embotech.com/FORCES-Pro (2014–2019)
  * [5] Garrett, C.R., Lozano-Perez, T., Kaelbling, L.P.: FFRob: Leveraging symbolic planning for efficient task and motion planning. The International Journal of Robotics Research 37(1), 104–136 (2018)
  * [6] Hauser, K.: Minimum constraint displacement motion planning. In: Robotics: science and systems. vol. 6, p. 2 (2013)
  * [7] Hauser, K.: The minimum constraint removal problem with three robotics applications. The International Journal of Robotics Research 33(1), 5–17 (2014)
  * [8] Kaelbling, L.P., Lozano-Pérez, T.: Integrated task and motion planning in belief space. The International Journal of Robotics Research 32(9-10), 1194–1227 (2013)
  * [9] Karami, H., Thomas, A., Mastrogiovanni, F.: A Task-Motion Planning Framework Using Iteratively Deepened AND/OR Graph Networks. arXiv preprint arXiv:2104.01549 (2021)
  * [10] Krontiris, A., Bekris, K.E.: Dealing with difficult instances of object rearrangement. In: Robotics: Science and Systems. vol. 1123 (2015)
  * [11] Latombe, J.C.: Robot Motion Planning. Kluwer Academic Publishers (1991)
  * [12] Nieuwenhuisen, D., van der Stappen, A.F., Overmars, M.H.: An effective framework for path planning amidst movable obstacles. In: Algorithmic Foundation of Robotics VII, pp. 87–102. Springer (2008)
  * [13] Reif, J.H.: Complexity of the mover’s problem and generalizations. In: 20th Annual Symposium on Foundations of Computer Science (sfcs 1979). pp. 421–427. IEEE Computer Society (1979)
  * [14] Srivastava, S., Fang, E., Riano, L., Chitnis, R., Russell, S., Abbeel, P.: Combined task and motion planning through an extensible planner-independent interface layer. In: Robotics and Automation (ICRA), IEEE International Conference on. pp. 639–646. IEEE (2014)
  * [15] Stilman, M., Kuffner, J.J.: Navigation among movable obstacles: Real-time reasoning in complex environments. International Journal of Humanoid Robotics 2(04), 479–503 (2005)
  * [16] Stilman, M., Schamburek, J.U., Kuffner, J., Asfour, T.: Manipulation planning among movable obstacles. In: Proceedings 2007 IEEE international conference on robotics and automation. pp. 3327–3332. IEEE (2007)
  * [17] Thomas, A., Mastrogiovanni, F., Baglietto, M.: Towards Multi-Robot Task-Motion Planning for Navigation in Belief Space. In: European Starting AI Researchers’ Symposium. CEUR (2020)
  * [18] Thomas, A., Mastrogiovanni, F., Baglietto, M.: MPTP: Motion-planning-aware task planning for navigation in belief space. Robotics and Autonomous Systems 141, 103786 (2021), https://www.sciencedirect.com/science/article/pii/S0921889021000713
  * [19] Van Den Berg, J., Stilman, M., Kuffner, J., Lin, M., Manocha, D.: Path planning among movable obstacles: a probabilistically complete approach. In: Workshop on the Algorithmic Foundations of Robotics VIII, WAFR, Guanajuato, Mexico. pp. 599–614. Springer (2009)
  * [20] Wilfong, G.: Motion planning in the presence of movable obstacles. In: Proceedings of the Fourth Annual Symposium on Computational Geometry. p. 279–288. SCG ’88, Association for Computing Machinery, New York, NY, USA (1988), https://doi.org/10.1145/73393.73422
  * [21] Zanelli, A., Domahidi, A., Jerez, J., Morari, M.: FORCES NLP: an efficient implementation of interior-point methods for multistage nonlinear nonconvex programs. International Journal of Control 93(1), 13–29 (2020)