• documentation
main content

plan motion for rigid body tree using bidirectional rrt -凯发k8网页登录

plan motion for rigid body tree using bidirectional rrt

since r2020b

description

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 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 object function. after planning, you can interpolate states along the path using the object function. to attempt to shorten the path by trimming edges, use the object function.

to specify a region to sample end-effector poses near the goal configuration, create a object and specify it as the goalregion input to the object function. to change the probability of sampling additional goal configurations, specify the workspacegoalregionbias property.

for more information about the computational complexity, see planning complexity.

creation

syntax

rrt = manipulatorrrt(robotrbt,{})
rrt = manipulatorrrt(robotrbt,collisionobjects)
rrt = manipulatorrrt(___,map=map)

description

example

rrt = manipulatorrrt(robotrbt,{}) creates a bidirectional rrt planner for the specified 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.

example

rrt = manipulatorrrt(___,map=map) specifies a 3-d occupancy map map to represent the environment. this requires navigation toolbox™.

input arguments

3-d occupancy map representing the environment, specified as a (navigation toolbox) object. note that the manipulatorrrt object does not deep copy the specified map and instead holds the handle to the specified map.

specifying this input argument requires navigation toolbox.

properties

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. during the extension process, this is the maximum distance a configuration can change.

when revolute joints have infinite limits, differences between two joint positions are calculated using the angdiff function.

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

probability to sample a goal state from the workspace goal region, specified as a positive value in the range [0,1). the bias defines the probability to add additional goal states to the tree from the object. when this value is set to zero, the workspacegoalregion object still samples a single goal for the planner to plan to.

increasing this value increases the likelihood of reaching a goal state in the goal region, but may lead to longer planning times because each new goal state adds additional complexity for planning.

dependency

you must use the goalregion input when calling the object function.

data types: double

ignore self collisions during planning, specified as a logical. if this property is set to true, the function skips checking between bodies for collisions and only compares the bodies to the environment. by not checking for self-collisions, you may improve the speed of the planning phase.

data types: logical

body pairs skipped for checking self-collisions, specified as either "parent" or "adjacent":

  • "parent" — skip collision checking between child and parent bodies.

  • "adjacent" — skip collision checking between bodies on adjacent indices.

see for more information.

data types: char | string

object functions

plan path using rrt for manipulators
interpolate states along path from rrt
trim edges to shorten path from rrt

examples

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];
show(robot);
hold on
show(env{1})
show(env{2})

figure contains an axes object. the axes object with xlabel x, ylabel y contains 31 objects of type patch, line. these objects represent world, iiwa_link_0, iiwa_link_1, iiwa_link_2, iiwa_link_3, iiwa_link_4, iiwa_link_5, iiwa_link_6, iiwa_link_7, iiwa_link_ee, iiwa_link_ee_kuka, iiwa_link_0_mesh, iiwa_link_1_mesh, iiwa_link_2_mesh, iiwa_link_3_mesh, iiwa_link_4_mesh, iiwa_link_5_mesh, iiwa_link_6_mesh, iiwa_link_7_mesh.

create the rrt planner for the robot model.

rrt = manipulatorrrt(robot,env);
rrt.skippedselfcollisions = "parent";

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.

rng(0)
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);
clf
for i = 1:20:size(interppath,1)
    show(robot,interppath(i,:));
    hold on
end
show(env{1})
show(env{2})
hold off

figure contains an axes object. the axes object with xlabel x, ylabel y contains 437 objects of type patch, line. these objects represent world, iiwa_link_0, iiwa_link_1, iiwa_link_2, iiwa_link_3, iiwa_link_4, iiwa_link_5, iiwa_link_6, iiwa_link_7, iiwa_link_ee, iiwa_link_ee_kuka, iiwa_link_0_mesh, iiwa_link_1_mesh, iiwa_link_2_mesh, iiwa_link_3_mesh, iiwa_link_4_mesh, iiwa_link_5_mesh, iiwa_link_6_mesh, iiwa_link_7_mesh.

specify a goal region in your workspace and plan a path within those bounds. the workspacegoalregion object defines the bounds on the xyz-position and zyx euler orientation of the robot end effector. the manipulatorrrt object plans a path based on that goal region and samples random poses within the bounds.

load an existing robot model as a rigidbodytree object.

robot = loadrobot("kinovagen3", "dataformat", "row");
ax = show(robot);

figure contains an axes object. the axes object with xlabel x, ylabel y contains 25 objects of type patch, line. these objects represent base_link, shoulder_link, halfarm1_link, halfarm2_link, forearm_link, wrist1_link, wrist2_link, bracelet_link, endeffector_link, shoulder_link_mesh, halfarm1_link_mesh, halfarm2_link_mesh, forearm_link_mesh, wrist1_link_mesh, wrist2_link_mesh, bracelet_link_mesh, base_link_mesh.

create path planner

create a rapidly-exploring random tree (rrt) path planner for the robot. this example uses an empty environment, but this workflow also works well with cluttered environments. you can add collision objects to the environment like the collisionbox or collisionmesh object.

planner = manipulatorrrt(robot,{});
planner.skippedselfcollisions="parent";

define goal region

create a workspace goal region using the end-effector body name of the robot.

define the goal region parameters for your workspace. the goal region includes a reference pose, xyz-position bounds, and orientation limits on the zyx euler angles. this example specifies bounds on the xy-plane in meters and allows rotation about the z-axis in radians.

goalregion = workspacegoalregion(robot.bodynames{end}); 
goalregion.referencepose = trvec2tform([0.5 0.5 0.2]);
goalregion.bounds(1, :) = [-0.2 0.2];    % x bounds
goalregion.bounds(2, :) = [-0.2 0.2];    % y bounds
goalregion.bounds(4, :) = [-pi/2 pi/2];  % rotation about the z-axis

you can also apply a fixed offset to all poses sampled within the region. this offset can account for grasping tools or variations in dimensions within your workspace. for this example, apply a fixed transformation that places the end effector 5 cm above the workspace.

goalregion.endeffectoroffsetpose = trvec2tform([0 0 0.05]);
hold on
show(goalregion);

figure contains an axes object. the axes object with xlabel x, ylabel y contains 35 objects of type line, patch. these objects represent base_link, shoulder_link, halfarm1_link, halfarm2_link, forearm_link, wrist1_link, wrist2_link, bracelet_link, endeffector_link, shoulder_link_mesh, halfarm1_link_mesh, halfarm2_link_mesh, forearm_link_mesh, wrist1_link_mesh, wrist2_link_mesh, bracelet_link_mesh, base_link_mesh.

plan path to goal region

plan a path to the goal region from the robot's home configuration. due to the randomness in the rrt algorithm, this example sets the rng seed to ensure repeatable results.

rng(0)
path = plan(planner,homeconfiguration(robot),goalregion);

show the robot executing the path. to visualize a more realistic path, interpolate points between path configurations.

interpconfigurations = interpolate(planner,path,5);
for i = 1 : size(interpconfigurations)
    show(robot,interpconfigurations(i,:),"preserveplot",false);
    set(ax,'zlim',[-0.05 0.75],'ylim',[-0.05 1],'xlim',[-0.05 1],...
        'cameraviewangle',5)
  
    drawnow
end
hold off

figure contains an axes object. the axes object with xlabel x, ylabel y contains 35 objects of type line, patch. these objects represent base_link, shoulder_link, halfarm1_link, halfarm2_link, forearm_link, wrist1_link, wrist2_link, bracelet_link, endeffector_link, shoulder_link_mesh, halfarm1_link_mesh, halfarm2_link_mesh, forearm_link_mesh, wrist1_link_mesh, wrist2_link_mesh, bracelet_link_mesh, base_link_mesh.

adjust end-effector pose

notice that the robot arm approaches the workspace from the bottom. to flip the orientation of the final position, add a pi rotation to the y-axis for the reference pose. 

goalregion.endeffectoroffsetpose = ...
    goalregion.endeffectoroffsetpose*eul2tform([0 pi 0],"zyx");

replan the path and visualize the robot motion again. the robot now approaches from the top.

hold on
show(goalregion);
path = plan(planner,homeconfiguration(robot),goalregion);
interpconfigurations = interpolate(planner,path,5);
for i = 1 : size(interpconfigurations)
    show(robot, interpconfigurations(i, :),"preserveplot",false);
    set(ax,'zlim',[-0.05 0.75],'ylim',[-0.05 1],'xlim',[-0.05 1])
    drawnow;
end
hold off

figure contains an axes object. the axes object with xlabel x, ylabel y contains 45 objects of type line, patch. these objects represent base_link, shoulder_link, halfarm1_link, halfarm2_link, forearm_link, wrist1_link, wrist2_link, bracelet_link, endeffector_link, shoulder_link_mesh, halfarm1_link_mesh, halfarm2_link_mesh, forearm_link_mesh, wrist1_link_mesh, wrist2_link_mesh, bracelet_link_mesh, base_link_mesh.

load the kinova gen 3 robot.

rbt = loadrobot("kinovagen3",dataformat="row");

create a 3-d occupancy map and set the coordinate at [0.4 0.0 0.4] to occupied.

map = occupancymap3d(10);
map.setoccupancy([0.4 0.0 0.4],1);

display the robot in the map.

show(map);
hold on
show(rbt);
axis("equal")
xlim([-1,1.0])
ylim([-1,1.0])
zlim([-0.5,1.2])

define a start configuration.

startconfig = [2.2131,-1.3950,0.1618,0.2053,-0.1624,1.1684,-2.1886];

define a goal configuration that is the same as the start configuration except for the first joint.

goalconfig = startconfig;
goalconfig(1) = 3.4;

create the manipulator rrt planner for the robot and specify the map as the environment using the map argument.

planner = manipulatorrrt(rbt,{},map=map);
planner.validationdistance=0.1;
planner.maxconnectiondistance=0.2;
planner.skippedselfcollisions="parent";

plan a path between the start and goal configuration. then interpolate between the paths.

plannedpath = plan(planner,startconfig,goalconfig);
interpoalatedpath = interpolate(planner,plannedpath);

animate the robot following the path.

rc=ratecontrol(10);
view([pi/3,pi/2,pi/4]);
for i = 1:size(interpoalatedpath,1)
    show(rbt,interpoalatedpath(i,:),fastupdate=true,preserveplot=false);
    waitfor(rc);
end

figure contains an axes object. the axes object with title occupancy map, xlabel x [meters], ylabel y [meters] contains 26 objects of type patch, line.

tips

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 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 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.

references

[1] 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™.

version history

introduced in r2020b

see also

objects

  • | | | (navigation toolbox)

functions

  • | |

topics

    网站地图