isStateValid
Description
checks if given states, or joint configurations, are valid for the rigid body tree robot
model specified by the state validator isValid
= isStateValid(manipSV
,states
)manipSV
. This object function
checks for self-collisions and collisions with the environment based on the properties of
the state validator.
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
Input Arguments
manipSV
— Manipulator state validator
manipulatorCollisionBodyValidator
object
Manipulator state validator, specified as a manipulatorCollisionBodyValidator
object, which is a subclass of nav.StateValidator
(Navigation Toolbox). The state validator contains properties that determine
the behavior of this function and isMotionValid
.
states
— Robot states in joint space
m-by-n matrix
Robot states in the joint space, specified as an
m-by-n matrix of joint positions.
m is the total number of states and n is the
number of nonfixed joints in the rigidBodyTree
robot model.
Data Types: double
Output Arguments
isValid
— Validity of state
m-element vector of logical scalars
Validity of input states returned as an m-element vector of
logical scalars, where m is the total number of states. A state is
valid, 1
or true
, if it does not result in
self-collisions, collisions with the environment, and it is within state bounds.
Otherwise the state is invalid, 0
or false
.
Data Types: logical
Version History
Introduced in R2021bR2023b: 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.
See Also
Objects
Functions
MATLAB 命令
您点击的链接对应于以下 MATLAB 命令:
请在 MATLAB 命令行窗口中直接输入以执行命令。Web 浏览器不支持 MATLAB 命令。
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)