# Moving Furniture in a Cluttered Room with RRT

This example shows how to plan a path to move bulky furniture in a tight space avoiding poles. This example shows a workflow of the "Piano Mover's Problem", which is used for testing path planning algorithms with constrained state spaces. This example uses the plannerRRTStar object to implement a custom optimized rapidly-exploring tree (RRT*) algoirthm. Provided example helpers illustrate how to define custom state spaces and state valdiation for any motion planning application.

### Model the Scene

To help visualize and solve this path planning problem, two helper classes are provided, ExampleHelperFurniture and ExampleHelperRoom. The ExampleHelperFurniture class assembles the furniture by putting three rectangle collision boxes together. The furniture is set up as below:

The ExampleHelperRoom defines the dimension of the room and provides functions to insert furniture into the room and to check whether the furniture is in collision with the walls or poles. These two classes are used for the state validator of the planner.

Show the room with a given length and width. Add a piece of furniture with a given pose.

len = 6;
wid = 2;
room = ExampleHelperRoom(len, wid);
chair = ExampleHelperFurniture;
show(room,gca)
axis equal

### Configure State Space

Create a stateSpaceSE2 object for the furniture. Set the bounds based on the room dimensions. The state space samples random states in the state space. In this example, the furniture state is a 3-element vector, [x y theta], for the xy-coordinates and angle of rotation in radians.

bounds = [-0.8 0.8; [-1 5]; [-pi pi]];

ss = stateSpaceSE2(bounds);
ss.WeightTheta = 2;

### Create a Custom State Validator

The planner requires a customized state validator to enable collision checking between furniture and the fixtures in the room. The provided class, ExampleHelperFurnitureInRoomValidator, checks the validity of the furniture states based on the pairwise convex polygon collision checking functions. The class automatically creates a room and puts the weird-shaped furniture in it at construction.

% Set the initial pose of the furniture
initPose = trvec2tform([0 3.5 0]);

% Create a customized state validator
sv = ExampleHelperFurnitureInRoomValidator(ss, initPose);

% Reduce the validation distance
% Validation distance determines the granularity of interpolation when
% checking the motion that connects two states.
sv.ValidationDistance = 0.1;

### Configure the Path Planner

Use plannerRRTStar as the planner and specify the custom state space and state validator. Specify additional parameters for the planner. The GoalReached example helper function returns true when a feasible path gets close enough to the goal within a threshold. This exits the planner.

% Create the planner
rrt = plannerRRTStar(ss, sv);

% Set ball radius for searching near neighbors

% Exit as soon as a path is found
rrt.ContinueAfterGoalReached = false;

% The motion length between two furniture poses should be less than 0.4 m
rrt.MaxConnectionDistance = 0.4;

% Increase the max iterations
rrt.MaxIterations = 20000;

% Use a customized goal function
rrt.GoalReachedFcn = @exampleHelperGoalFunc;

### Plan the Move

Set a start and end pose for the furniture. This example moves from one pole to the other and rotates the chair pi radians. Plan the path between poses. Visualize the search tree and final path.

% Set the init and goal poses
start = [0 3.5 0];
goal = [0 -0.2 pi];

% Set random number seed for repeatability
rng(0, 'twister');
[path, solnInfo] = plan(rrt,start,goal);

hold on
% Search tree
plot(solnInfo.TreeData(:,1), solnInfo.TreeData(:,2), '.-');
% Interpolate path and plot points
interpolate(path,300)
plot(path.States(:,1), path.States(:,2), 'r-', 'LineWidth', 2)

hold off

### Visualize the Motion

An example helper is provided for smoothing the path by cutting corners of the path where possible. Animate the motion of the furniture from start to goal pose. The animation plot shows intermediate states as the furniture navigates to the goal position.

f = figure;

% Smooth the path, cut the corners wherever possible
pathSm = exampleHelperSmoothPath(path, sv);

interpolate(pathSm,100);
animateFurnitureMotion(sv.Room,1,pathSm.States, axes(f))

% show the trace of furniture
skip = 6;
states = pathSm.States([1:skip:end, pathSm.NumStates], :);
exampleHelperShowFurnitureTrace(sv.Room.FurnituresInRoom{1}, states);