sample
Description
samples an end-effector pose in the world frame as a homogeneous transformation matrix. eePose
= sample(goalRegion
)
The function returns a pose uniformly sampled within the Bounds property relative to the reference frame and applies the following transformations based on the ReferencePose and EndEffectorOffsetPose properties:
tSample; % Pose sampled within Bounds Tw0 = goalRegion.ReferencePose; TeW = goalRegion.EndEffectorOffsetPose; eePose = Tw0 * tSample * TeW; % tSample is a pose within the bounds.
samples multiple poses based on the input eePose
= sample(goalRegion
,numSamples
)numSamples
. The function
returns the end-effector poses as a 3-D array of homogeneous transforms.
Examples
Sample Multiple Poses In A Workspace Goal Region
Sample various poses within the bounds of a workspace goal region for a manipulator arm. Some end-effector poses may not be desirable due to the positioning of the arm bodies and obstacles in the scene. The workspaceGoalRegion
object defines the bounds on the xyz-position and zyx Euler orientation of the robot end effector. The sample
object function uniformly samples random poses within the bounds. Find configurations that achieve these end-effector poses and determine the best by visualization.
Load an existing robot model as a rigidBodyTree
object.
robot = loadrobot("kinovaGen3","DataFormat","row"); show(robot,"Collisions","on","Visuals","off");
Add a can as a collisionCylinder
object to the robot arm.
can = collisionCylinder(0.05, 0.1);
can.Pose = trvec2tform([0.2, 0.3, 0.5]);
addCollision(robot.Bodies{end},"cylinder", [0.05, 0.1], trvec2tform([0, 0, 0.02]));
The goal of this example is to place this can on a table with other cans. Add the table and other cans to the environment by creating a cell array of collision objects. Show the entire env
cell array.
table = collisionBox(0.7, 0.5, 0.04); table.Pose = trvec2tform([0, 0.5, 0.43]); env = {can, copy(can), copy(can), table}; env{2}.Pose = trvec2tform([-0.1, 0.3, 0.5]); env{3}.Pose = trvec2tform([-0.1, 0.5, 0.5]); hold on for i = 1: length(env) show(env{i}) end show(robot,homeConfiguration(robot),"Collisions","on","Visuals","off");
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 xyz bounds within the table dimensions and fixes rotation to a small range in the y- and x-axis.
tableRegion = workspaceGoalRegion("EndEffector_Link",... "ReferencePose",table.Pose); tableRegion.EndEffectorOffsetPose(1:3,1:3) = eul2rotm([0, 0, pi]); tableRegion.EndEffectorOffsetPose(3, end) = 0.1; tableRegion.Bounds = ... [-table.X/2, table.X/2; % X Bounds -table.Y/2, table.Y/2; % Y Bounds 0.04, 0.10; % Z Bounds -pi, pi; % Rotation about Z-axis -0.01, 0.01; % Y-Axis -0.01, 0.01;]; % X-Axis show(tableRegion); view(165,50) camzoom(3.5)
Sample Poses
Uniformly sample poses within the table region using the sample
object function. In this example, set the rng
seed to get repeatable results. Create vectors for storing valid and invalid poses.
rng(0) poses = sample(tableRegion,10); validPoses = []; invalidPoses = [];
Check for Collisions
To find configurations for those poses, create an inverse kinematics (IK) solver.
ik = inverseKinematics('RigidBodyTree',robot);
config = cell(10);
Test the sampled poses by iterating through the sampled poses, solving for configurations using IK, and checking for collisions. Show the valid configurations.
for i = 1:length(poses) % Solve for robot configuraiton using IK. config{i} = ik("EndEffector_Link",poses(:,:,i),ones(6,1),homeConfiguration(robot)); % Check for collisions. isColliding = checkCollision(robot,config{i},env,SkippedSelfCollisions="parent"); if ~isColliding % If not in collision, show robot configuration and save valid pose. show(robot,config{i},"PreservePlot",false,"Collisions","on","Visuals","off"); drawnow validPoses = [validPoses; i]; else invalidPoses = [invalidPoses; i]; end end
disp(string(validPoses'))
"3" "5" "7" "10"
Visualize A Singe Valid Pose
Plot all valid poses as transforms. The final valid configuration from checking collisions is still visible in the figure.
translations = tform2trvec(poses(:,:,validPoses));
rotations = tform2quat(poses(:,:,validPoses));
plotTransforms(translations,rotations,"FrameSize",0.1)
Show a valid configuration from the list. Change the index in validPoses
to look at different poses. Call hold off
to stop preserving figure elements. To manually inspect poses and configurations, comment out the final line when running.
poseIndex = validPoses(1); show(robot,config{poseIndex},"PreservePlot",false,"Collisions","on","Visuals","off"); hold off
Input Arguments
goalRegion
— Workspace goal region
workspaceGoalRegion
object
Workspace goal region, specified as a workspaceGoalRegion
object.
numSamples
— Number of samples
positive integer
Number of samples, specified as a positive integer
Output Arguments
eePose
— Poses sampled within workspace bounds
4-by-4 homogeneous transform matrix | four-by-four-by-n array
Poses sampled within the workspace bounds in the world frame, returned as a
four-by-four homogeneous transformation matrix or 4-by-4-by-n array,
where n is the number of samples
numSamples
.
The function returns a pose uniformly sampled within the Bounds property relative to the reference frame and applies the following transformations based on the ReferencePose and EndEffectorOffsetPose properties:
tSample = rand(6,2); Tw0 = goalRegion.ReferencePose; TeW = goalRegion.EndEffectorOffsetPose; eePose = Tw0 * tSample * TeW;
Data Types: double
Extended Capabilities
C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.
Version History
Introduced in R2021a
See Also
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)