manipulatorCollisionBodyValidator

Validate states for collision bodies of rigid body tree

Since R2021b

Description

The manipulatorCollisionBodyValidator object performs state and motion validity checks for a rigid body tree robot model. To check if the collision bodies collide either with other bodies (self-collisions) or the environment, use the isStateValid object function. To check if a motion between two states is valid, use the isMotionValid object function.

This object requires Navigation Toolbox™.

Creation

Syntax

manipSV = manipulatorCollisionBodyValidator

manipSV = manipulatorCollisionBodyValidator(ss)

manipSV = manipulatorCollisionBodyValidator(ss,Name=Value)

Description

manipSV = manipulatorCollisionBodyValidator creates a state validator with default values for a manipulatorStateSpace object.

manipSV = manipulatorCollisionBodyValidator(ss) creates a state validator for a manipulatorStateSpace object that represents a robot model state space and contains collision bodies for rigid body elements. Specify ss as a manipulatorStateSpace object.

manipSV = manipulatorCollisionBodyValidator(ss,Name=Value) specifies Properties as name-value arguments

Properties

ValidationDistance — Distance resolution for motion validation

0.1 (default) | positive scalar in meters

Distance resolution for motion validation, specified as a positive scalar. The validation distance determines the number of interpolated states between states specified to the isMotionValid object function. The object function validates each interpolated state by checking for collisions with the robot and the environment.

Data Types: double

IgnoreSelfCollision — Ignore self collisions toggle

0 or false (default) | 1 or true

Ignore self collisions toggle, specified as a logical. If this property is set to true, the isMotionValid object function skips checking between bodies for collisions and only compares the bodies to the environment. Not checking for self-collisions can improve the speed of the planning phase, but your state space should contain joint limits that ensure self-collisions are not possible.

Data Types: logical

Environment — Collision objects in robot environment

{} (default) | cell array of collision body objects

Collision objects in the robot environment, specified as a cell array of collision objects of these types:

• collisionBox

• collisionCylinder

• collisionMesh

• collisionSphere

box = collisionBox(0.1,0.1,0.5); % XYZ Lengths
box.Pose = trvec2tform([0.2 0.2 0.5]); % XYZ Position
sphere = collisionSphere(0.25); % Radius
sphere.Pose = trvec2tform([-0.2 -0.2 0.5]); % XYZ Position
env = {box sphere};
manipSS = manipulatorCollisionBodyValidator(ss,Environment=env);

Data Types: logical

StateSpace — Manipulator state space

manipulatorStateSpace object

Manipulator state space, specified as a manipulatorStateSpace object, which is a subclass of nav.StateSpace (Navigation Toolbox).

SkippedSelfCollisions — Body pairs skipped for checking self-collisions

"parent" (default) | "adjacent" | p-by-2 cell array of character vectors

Body pairs skipped for checking self-collisions, specified as "parent", "adjacent", or a p-by-2 cell array of character vectors:

• "parent" — Skip collision checking between child and parent bodies. See Skip Self-Collision Checking Between Parent and Adjacent Bodies for more information.

• "adjacent" — Skip collision checking between bodies on adjacent indices. See Skip Self-Collision Checking Between Parent and Adjacent Bodies for more information.

• p-by-2 cell array of character vectors — Skip collision checking between specific body pairs. Each row specifies a pair of body names between which to skip self-collision checking. p is the number of body pairs to skip for self-collision checking. For more information, see Skip Self-Collision Checking Between Specific Body Pairs.

Data Types: char | string

Object Functions

isStateValid	Check if state is valid
isMotionValid	Check if path between states is valid

Examples

Validate State and Motion Manipulator State Space

Generate states to form a path, validate motion between states, and check for self-collisions and environmental collisions with objects in your world. The manipulatorStateSpace object represents the joint configuration space of your rigid body tree robot model, and can sample states, calculate distances, and enforce state bounds. The manipulatorCollisionBodyValidator object validates the state and motion based on the collision bodies in your robot model and any obstacles in your environment.

Load Robot Model

Use the loadrobot function to access predefined robot models. This example uses the Quanser QArm™ robot and joint configurations are specified as row vectors.

robot = loadrobot("quanserQArm",DataFormat="row");
figure(Visible="on")
show(robot);
xlim([-0.5 0.5])
ylim([-0.5 0.5])
zlim([-0.25 0.75])
hold on

Configure State Space and State Validation

Create the state space and state validator from the robot model.

ss = manipulatorStateSpace(robot);
sv = manipulatorCollisionBodyValidator(ss,SkippedSelfCollisions="parent");

Set the validation distance to 0.05, which is based on the distance normal between two states. You can configure the validator to ignore self collisions to improve the speed of validation, but must consider whether your robot model has the proper joint limit settings set to ensure it does not collide with itself.

sv.ValidationDistance = 0.05;
sv.IgnoreSelfCollision = true;

Place collision objects in the robot environment. Set the Environment property of the collision validator object using a cell array of objects.

box = collisionBox(0.1,0.1,0.5); % XYZ Lengths
box.Pose = trvec2tform([0.2 0.2 0.5]); % XYZ Position
sphere = collisionSphere(0.25); % Radius
sphere.Pose = trvec2tform([-0.2 -0.2 0.5]); % XYZ Position
env = {box sphere};
sv.Environment = env;

Visualize the environment.

for i = 1:length(env)
    show(env{i})
end
view(60,10)

Plan Path

Start with the home configuration as the first point on the path.

rng(0); % Repeatable results
start = homeConfiguration(robot);
path = start;
idx = 1;

Plan a path using these steps, in a loop:

• Sample a nearby goal configuration, using the Gaussian distribution, by specifying the standard deviation for each joint angle.

• Check if the sampled goal state is valid.

• If the sampled goal state is valid, check if the motion between states is valid and, if so, add it to the path.

for i = 2:25
    goal = sampleGaussian(ss,start,0.25*ones(4,1));
    validState = isStateValid(sv,goal);
    
    if validState % If state is valid, check motion between states.
        [validMotion,~] = isMotionValid(sv,path(idx,:),goal);

        if validMotion % If motion is valid, add to path.
            path = [path; goal];
            idx = idx + 1;
        end
    end
end

Visualize Path

After generating the path of valid motions, visualize the robot motion. Because you sampled random states near the home configuration, you should see the arm move around that initial configuration.

To visualize the path of the end effector in 3-D, get the transformation, relative to the base world frame at each point. Store the points as an xyz translation vector. Plot the path of the end effector.

eePose = nan(3,size(path,1));

for i = 1:size(path,1)
    show(robot,path(i,:),PreservePlot=false);
    eePos(i,:) = tform2trvec(getTransform(robot,path(i,:),"END-EFFECTOR")); % XYZ translation vector
    plot3(eePos(:,1),eePos(:,2),eePos(:,3),"-b",LineWidth=2)
    drawnow
end

Version History

Introduced in R2021b

R2023b: Specify each body pair to skip for self-collision checking

You can now select which body pairs to skip in self-collision checks by specifying the SkippedSelfCollisions name-value argument as a cell array of body pairs. For more information, see the SkippedSelfCollisions property.

R2023b: Improved Performance of Object Functions

These object functions of the manipulatorCollisionBodyValidator object have improved performance in MATLAB^® R2023b interpreted execution compared to R2023a. For example, this code is about 4 times faster than in the previous release:

function timingtest
    rng(10);
    kuka=loadrobot("kukaIiwa14",DataFormat="row");
    env={collisionBox(0.5,0.5,0.05),...
        collisionSphere(0.3)};
    env{1}.Pose=trvec2tform([0,0,-0.05]);
    env{2}.Pose=trvec2tform([0.1,0.2,0.8]);
    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];
    ss=manipulatorStateSpace(kuka);
    sv=manipulatorCollisionBodyValidator(ss,Environment=env,SkippedSelfCollisions="parent",ValidationDistance=0.01);
    tic;
    isvalid=isStateValid(sv,sampleUniform(ss,1e4));
    isvalid=isMotionValid(sv,startConfig,goalConfig);
    toc
end

The approximate execution times are:

R2023a: 68.1 s

R2023b: 18.0 s

This code was timed on a Windows 10 system with a 3.60 GHz Intel^® W-2133 CPU with 64 GB of RAM.

R2022b: Alter rigid body tree self-collision checking behavior change and new default self-collision checking behavior

You can now specify self-collision checking behavior for a rigid body tree robot model by using the SkippedSelfCollisions property. Specify SkippedSelfCollisions as "parent" or "adjacent":

• "parent" — Collision checking ignores self-collisions between parent and child rigid bodies.

• "adjacent" — Collision checking ignores self -collisions between rigid bodies of adjacent indices.

As of R2022b, the default behavior of collision checking is to ignore self-collisions between parent and child rigid bodies. In previous releases, the default behavior of self-collision checking was to ignore self-collisions between adjacent rigid bodies. To instead ignore self-collisions between rigid bodies of adjacent indices, specify SkippedSelfCollisions as "adjacent".

See Skip Self-Collision Checking Between Parent and Adjacent Bodies for more information.