manipulatorCollisionBodyValidator
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
Description
creates a state validator with default values for a manipSV
= manipulatorCollisionBodyValidatormanipulatorStateSpace
object.
creates a state validator for a manipSV
= manipulatorCollisionBodyValidator(ss
)manipulatorStateSpace
object that represents a robot model state space and
contains collision bodies for rigid body elements. Specify ss
as a
manipulatorStateSpace
object.
specifies Properties as name-value argumentsmanipSV
= manipulatorCollisionBodyValidator(ss
,Name=Value
)
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:
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 R2021bR2023b: 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.
See Also
Objects
Functions
MATLAB Command
You clicked a link that corresponds to this MATLAB command:
Run the command by entering it in the MATLAB Command Window. Web browsers do not support MATLAB commands.
Select a Web Site
Choose a web site to get translated content where available and see local events and offers. Based on your location, we recommend that you select: .
You can also select a web site from the following list:
How to Get Best Site Performance
Select the China site (in Chinese or English) for best site performance. Other MathWorks country sites are not optimized for visits from your location.
Americas
- América Latina (Español)
- Canada (English)
- United States (English)
Europe
- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)
- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)