Main Content

sample

Sample end-effector poses in world frame

Description

eePose = sample(goalRegion) samples an end-effector pose in the world frame as a homogeneous transformation matrix.

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.

example

eePose = sample(goalRegion,numSamples) samples multiple poses based on the input numSamples. The function returns the end-effector poses as a 3-D array of homogeneous transforms.

Examples

collapse all

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"); 

Figure contains an axes object. The axes object contains 55 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, Shoulder_Link_coll_mesh, HalfArm1_Link_coll_mesh, HalfArm2_Link_coll_mesh, ForeArm_Link_coll_mesh, Wrist1_Link_coll_mesh, Wrist2_Link_coll_mesh, Bracelet_Link_coll_mesh, base_link_coll_mesh, EndEffector_Link_coll_mesh.

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 specifiies 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)

Figure contains an axes object. The axes object contains 65 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, Shoulder_Link_coll_mesh, HalfArm1_Link_coll_mesh, HalfArm2_Link_coll_mesh, ForeArm_Link_coll_mesh, Wrist1_Link_coll_mesh, Wrist2_Link_coll_mesh, Bracelet_Link_coll_mesh, base_link_coll_mesh, EndEffector_Link_coll_mesh.

Sample Poses

Uniformly sample poses within the table region using the sample object function. In this example, set the rng seed to get repeatible 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);
    
    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

Figure contains an axes object. The axes object contains 40 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, Shoulder_Link_coll_mesh, HalfArm1_Link_coll_mesh, HalfArm2_Link_coll_mesh, ForeArm_Link_coll_mesh, Wrist1_Link_coll_mesh, Wrist2_Link_coll_mesh, Bracelet_Link_coll_mesh, EndEffector_Link_coll_mesh, base_link_coll_mesh.

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)

Figure contains an axes object. The axes object contains 56 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, Shoulder_Link_coll_mesh, HalfArm1_Link_coll_mesh, HalfArm2_Link_coll_mesh, ForeArm_Link_coll_mesh, Wrist1_Link_coll_mesh, Wrist2_Link_coll_mesh, Bracelet_Link_coll_mesh, EndEffector_Link_coll_mesh, base_link_coll_mesh.

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

Figure contains an axes object. The axes object contains 56 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, Shoulder_Link_coll_mesh, HalfArm1_Link_coll_mesh, HalfArm2_Link_coll_mesh, ForeArm_Link_coll_mesh, Wrist1_Link_coll_mesh, Wrist2_Link_coll_mesh, Bracelet_Link_coll_mesh, EndEffector_Link_coll_mesh, base_link_coll_mesh.

Input Arguments

collapse all

Workspace goal region, specified as a workspaceGoalRegion object.

Number of samples, specified as a positive integer

Output Arguments

collapse all

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

Introduced in R2021a