Main Content


Plan motion for rigid body tree using bidirectional RRT


The manipulatorRRT object is a single-query planner for manipulator arms that uses the bidirectional rapidly exploring random trees (RRT) algorithm with an optional connect heuristic to potentially increase speed.

The bidirectional RRT planner creates two trees with root nodes at the specified start and goal configurations. To extend each tree, the planner generates a random configuration and, if valid, takes a step from the nearest node based on the MaxConnectionDistance property. After each extension, the planner attempts to connect between the two trees using the new extension and the closest node on the opposite tree. Invalid configurations or connections that collide with the environment are not added to the tree.

For a greedier search, enabling the EnableConnectHeuristic property disables the limit on the MaxConnectionDistance property when connecting between the two trees.

Image showing the extension of the two branching trees from start and end goal. When the EnableConnectHeurist is true, the connection steps are not limited by the max connection distance.

Setting the EnableConnectHueristic property to false limits the extension distance when connecting between the two trees to the value of the MaxConnectionDistance property.

Image showing the extension of the two branching trees from start and end goal. When the EnableConnectHeurist is false, the connection steps are limited by the max connection distance.

The object uses a rigidBodyTree robot model to generate the random configurations and intermediate states between nodes. Collision objects are specified in the robot model to validate the configurations and check for collisions with the environment or the robot itself.

To plan a path between a start and a goal configuration, use the plan object function. After planning, you can interpolate states along the path using the interpolate object function. To attempt to shorten the path by trimming edges, use the shorten object function.

For more information about the computation complexity, see Planning Complexity.




rrt = manipulatorRRT(robotRBT,{}) creates a bidirectional RRT planner for the specified rigidBodyTree robot model. The empty cell array indicates that there are no obstacles in the environment.

rrt = manipulatorRRT(robotRBT,collisionObjects) creates a planner for a robot model with collision objects placed in the environment. The planner checks for collisions with these objects.


expand all

Maximum length between planned configurations, specified as a positive scalar. The object computes the length of the motion as the Euclidean distance between the two joint configurations. Differences between two joint positions for a revolute joint are calculated using the angdiff function. During the extension process, this is the maximum distance a configuration can change.

If the EnableConnectheuristic property is set to true, the object ignores this distance when connecting the two trees during the connect stage.

Data Types: double

Distance resolution for validating motion between configurations, specified as a positive scalar. The validation distance determines the number of interpolated nodes between two adjacent nodes in the tree. The object validates each interpolated node by checking for collisions with the robot and the environment.

Data Types: double

Maximum number of random configurations generated, specified as a positive integer.

Data Types: double

Directly join trees during the connect phase of the planner, specified as a logical 1 (true) or 0 (false). Setting this property to true causes the object to ignore the MaxConnectionDistance property when attempting to connect the two trees together.

Data Types: logical

Object Functions

planPlan path using RRT for manipulators
interpolateInterpolate states along path from RRT
shortenTrim edges to shorten path from RRT


collapse all

Use the manipulatorRRT object to plan a path for a rigid body tree robot model in an environment with obstacles. Visualize the planned path with interpolated states.

Load a robot model into the workspace. Use the KUKA LBR iiwa© manipulator arm.

robot = loadrobot("kukaIiwa14","DataFormat","row");

Generate the environment for the robot. Create collision objects and specify their poses relative to the robot base. Visualize the environment.

env = {collisionBox(0.5, 0.5, 0.05) collisionSphere(0.3)};
env{1}.Pose(3, end) = -0.05;
env{2}.Pose(1:3, end) = [0.1 0.2 0.8];

hold on

Create the RRT planner for the robot model.

rrt = manipulatorRRT(robot,env);

Specify a start and a goal configuration.

startConfig = [0.08 -0.65 0.05 0.02 0.04 0.49 0.04];
goalConfig =  [2.97 -1.05 0.05 0.02 0.04 0.49 0.04];

Plan the path. Due to the randomness of the RRT algorithm, set the rng seed for repeatability.

path = plan(rrt,startConfig,goalConfig);

Visualize the path. To add more intermediate states, interpolate the path. By default, the interpolate object function uses the value of ValidationDistance property to determine the number of intermediate states. The for loop shows every 20th element of the interpolated path.

interpPath = interpolate(rrt,path);
for i = 1:20:size(interpPath,1)
    hold on
hold off


Planning Complexity

  • When planning the motion between nodes in the tree, a set of configurations are generated and validated. This computation time of the planner is proportional to the number of configurations generated. The number of configurations between nodes is controlled by the ratio of the MaxConnectionDistance and ValidationDistance properties. To improve planning time, consider increasing the validation distance or decreasing the max connection distance.

  • Validating each configuration has a complexity of O(mn+m2), where m is the number of collision bodies in the rigidBodyTree object and n is the number of collision objects in worldObjects. Using large numbers of meshes to represent your robot or environment increases the time for validating each configuration.

Infinite Joint Limits

  • If your rigidBodyTree robot model has joint limits that have infinite range (e.g. revolute joint with limits of [-Inf Inf]), the manipulatorRRT object uses limits of [-1e10 1e10] to perform uniform random sampling in the joint limits.


Kuffner, J. J., and S. M. LaValle. “RRT-Connect: An Efficient Approach to Single-Query Path Planning.” In Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No.00CH37065), 2:995–1001. San Francisco, CA, USA: IEEE, 2000. https://doi:10.1109/ROBOT.2000.844730.


Extended Capabilities

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.

Introduced in R2020b