【MATLAB示例】Sample Multiple Poses In A Workspace Goal Region在工作空间目标区域内采样多个姿态

在工作空间目标区域内采样多个姿态

为机械臂在工作空间目标区域内采样多个姿态。由于机械臂的各部分位置及场景中的障碍物，有些末端执行器的姿态可能不理想。workspaceGoalRegion 对象定义了机器人末端执行器在 xyz 位置和 zyx 欧拉角度下的范围。sample 对象函数在这些范围内均匀采样随机姿态。找到实现这些末端执行器姿态的配置，并通过可视化确定最佳姿态。

加载一个现有的机器人模型作为 rigidBodyTree 对象。

robot = loadrobot("kinovaGen3","DataFormat","row");
show(robot,"Collisions","on","Visuals","off");

将一个罐子作为 collisionCylinder 对象添加到机器人手臂中。

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

此示例的目标是将这个罐子放在有其他罐子的桌子上。通过创建碰撞对象的单元数组，将桌子和其他罐子添加到环境中，并显示整个 env 单元数组。

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

定义目标区域

使用机器人末端执行器的体名创建一个工作空间目标区域。定义工作空间的目标区域参数。目标区域包括参考姿态、xyz 位置范围以及 zyx 欧拉角度的方向限制。此示例在桌子尺寸范围内指定了 xyz 的边界，并将旋转固定在 y 轴和 x 轴的较小范围内。

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轴范围
    -table.Y/2, table.Y/2;  % Y轴范围
    0.04, 0.10;             % Z轴范围
    -pi, pi;                % Z轴旋转
    -0.01, 0.01;            % Y轴旋转
    -0.01, 0.01;];          % X轴旋转

显示目标区域。

show(tableRegion);
view(165,50)
camzoom(3.5)

采样姿态

使用 sample 对象函数在桌子区域内均匀采样姿态。在此示例中，设置随机数种子以获取可重复的结果。创建用于存储有效和无效姿态的向量。

rng(0)
poses = sample(tableRegion,10);
validPoses = [];
invalidPoses = [];

检查碰撞

为了找到这些姿态的配置，创建一个逆运动学 (IK) 解算器。

ik = inverseKinematics('RigidBodyTree',robot);
config = cell(10);

通过迭代采样姿态，使用 IK 求解配置并检查碰撞，测试采样的姿态。显示有效的配置。

for i = 1:length(poses)
    % 使用IK求解机械臂配置。
    config{i} = ik("EndEffector_Link",poses(:,:,i),ones(6,1),homeConfiguration(robot));
    % 检查碰撞。
    isColliding = checkCollision(robot,config{i},env,SkippedSelfCollisions="parent");
    
    if ~isColliding % 如果没有碰撞，显示机械臂配置并保存有效姿态。
        show(robot,config{i},"PreservePlot",false,"Collisions","on","Visuals","off");
        drawnow
        validPoses = [validPoses; i];
    else
        invalidPoses = [invalidPoses; i];
    end
end
disp(string(validPoses'))

可视化单个有效姿态

将所有有效姿态绘制为变换矩阵。碰撞检查中最后一个有效的配置仍然可见在图形中。

translations = tform2trvec(poses(:,:,validPoses));
rotations = tform2quat(poses(:,:,validPoses));

plotTransforms(translations,rotations,"FrameSize",0.1)

显示有效配置列表中的一个。更改 validPoses 中的索引以查看不同的姿态。调用 hold off 停止保持图形元素。在运行时，若要手动检查姿态和配置，请注释掉最后一行。

poseIndex = validPoses(1);
show(robot,config{poseIndex},"PreservePlot",false,"Collisions","on","Visuals","off");
hold off