Matlab机械手碰撞检测应用

本文包含三个部分：

Matlab碰撞检测的实现
URDF文件的制作
机械手STL文件添加夹爪

一.Matlab碰撞检测的实现

首先上代码

%% 检测在结构环境中机器人是否与物体之间发生碰撞情况，如何避免？
% https://www.mathworks.com/help/robotics/ug/check-for-environmental-collisions-with-manipulators.html
%% 构建环境
% Create two platforms
clc
clear
platform1 = collisionBox(0.5,0.5,0.25);
platform1.Pose = trvec2tform([-1 0 0.2]);
worldCollisionArray = {platform1};
%% 创建figure对象显示环境：可以直接用exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);
ax=gca;
% 显示桌子1并上色
[~, patchObj]=show(platform1,'Parent', ax);
patchObj.FaceColor = [1 0.6 0.9];
axis([-0.8,1,-0.8,1.2,0,1.4])%调整图框范围
view(141,22)%调节视角
hold on
%% 添加机器人模型：可更改为DH方向构建。
robot=importrobot("E:\3D\3D抓取\yaskawaGP12\urdf\yaskawaGP12.urdf");
robot.Gravity=[0 0 -9.81];
robot.DataFormat="column";
%robot = loadrobot("kinovaGen3","DataFormat","column","Gravity",[0 0 -9.81]);
show(robot,homeConfiguration(robot),"Parent",ax, 'Frames','off');startConfig=[3;1;0.8;0;0;pi];%关结空间
endConfig=[3;1;0.2;0;0;pi];
endEffector="link6";
%% 使用梯形速度规划从起点到终点的轨迹【关节空间】
q= trapveltraj([startConfig,endConfig],60,"EndTime",2);hold on
axis([-1.8,1,-1.8,1.2,0,2.2])%调整图框范围
view(141,22)%调节视角
axis on
hold on
title('机器人根据起点和终点位姿直接梯形速度的运动')
for i = 1:2:length(q)show(robot,q(:,i),"PreservePlot",false);%false 不留下重影poseNow = getTransform(robot, q(:,i), endEffector);%正运动学plot3(poseNow(1,4), poseNow(2,4), poseNow(3,4),'b.','MarkerSize',5)drawnow
end%%
% 初始化输出
inCollision = false(length(q),1); %===> zeros(length(q),1)
isConfigInCollision=false(length(q),1);
worldCollisionPairIdx = cell(length(q),1); % 元胞数组，保存与环境碰撞的部件和关节配置的索引
for i = 1:length(q)isConfigInCollision(i) = checkCollision(robot,q(:,i),Exhaustive='on');[inCollision(i),sepDist] = checkCollision(robot,q(:,i),worldCollisionArray,"IgnoreSelfCollision","on","Exhaustive","on");%  sepDist为输出是机器人身体与世界碰撞对象之间的距离--->存储为矩阵[bodyIdx,worldCollisionObjIdx] = find(isnan(sepDist)); % 找到碰撞的部件。距离是空NaN，则发生碰撞worldCollidingPairs = [bodyIdx,worldCollisionObjIdx]; worldCollisionPairIdx{i} = worldCollidingPairs;  %机器人部件索引第一列，环境物体索引第二列
end
isSelfCollision = any(isConfigInCollision);
isTrajectoryInCollision = any(inCollision);
%% 检测到碰撞并可视化构型，第一个和最后一个检测到的碰撞
collidingIdx1 = find(inCollision,1);%第一个：构型索引19
if(collidingIdx1)%collidingIdx2 = find(inCollision,1,"last");%最后一个：构型索引172% Identify the colliding rigid bodies.找到机器人中的碰撞部分collidingBodies1 = worldCollisionPairIdx{collidingIdx1}*[1 0]';%取碰撞的机器人部件第一列的=矩阵乘以[1,0]'%具体看矩阵维度% collidingBodies2 = worldCollisionPairIdx{collidingIdx2}*[1 0]';%  Identify the colliding world bodies.找到环境中的碰撞部分collidingworld1 = worldCollisionPairIdx{collidingIdx1}*[0 1]';%取碰撞的机器人部件第一列的=矩阵乘以[1,0]'%具体看矩阵维度% collidingworld2 = worldCollisionPairIdx{collidingIdx2}*[0 1]';%% 显示环境，并将碰撞进行可视化exampleHelperHighlightCollisionBodies(robot,collidingBodies1 +1,ax);% hold on[~, patchObj]=show(worldCollisionArray{collidingworld1(1)},"Parent"',ax);patchObj.FaceColor = [1 0.8 0];title('发生碰撞的部分构型显示','Color', 'r')
elsetitle('未发生碰撞','Color', 'g')
end

代码中首先创建platform1箱体、robot机器手模型，后根据起点、终点规划轨迹，再图像化显示，然后进入正题判断碰撞，碰撞分自碰撞与外部碰撞；

此功能需要安装Robotics System Toolbox，下载路径：Robotics Toolbox - Peter Corke

下载后双击安装即可。

上面代码的urdf文件可以使用C:\Program Files\MATLAB\R2022a\toolbox\robotics\robotmanip\robotModels\roboturdf内的替代；

注意：loadrobot函数加载的模型文件与importrobot不同，后者需要URDF文件。

此部分可参考博客：Robotics System Toolbox中的机器人运动（6）-碰撞检测-CSDN博客

二.URDF文件制作

此处第一次接触，较艰难

参考博客：特别章节-0.1 SolidWorks导出机械臂的URDF模型各个关节坐标系设置_sw2urdf导入模型如何正确设置坐标系-CSDN博客

Solidworks_to_URDF导出教程 - 知乎

还有SW的安装：SolidWorks 2022 SP5.0 软件下载+安装教程XL

SW还需要安装转URDF插件：sw_urdf_exporter - ROS Wiki，下载后安装即可

补充：

注意图中每一处的选择，JointName内的名称不能与link1相同，JointType内需要选择对应部件，还有机械手是关节型的，link应一级一级展开

然后正常导出即可。

三.机械手STL文件添加夹爪

思路是把导出URDF里的STL文件link6加上夹爪就好，碰撞检测时就可以带夹爪判断了。

修改STL文件废了些时间，实际完全可以用Matlab写一个程序实现

目前用的时Meshlab软件修改，在文件-导入网格，打开对应文件。

右侧部件鼠标右击出现菜单，对部件的平移旋转等需要用矩阵操作，做这个的应该都了解矩阵

这就是操作界面，修改数值然后右下角Apply，注意前三个数值单位是米；

注意机械手上的link6部件不能动，动了其与机械手相对位置就不同了；

这样机械手抓取碰撞检测就完成了，后面还有Matlab导出DLL在C#语言调用的部分，问题不大，有一个Matlab函数返回多个参数需要在函数第一个参数设置返回数量，超过1则返回MWArray[]

RobotCollision matlab = new RobotCollision();
MWArray robot;
public Form1()
{InitializeComponent();robot = matlab.MyLoadRobot(@"E:\3D\3D抓取\yaskawaGP12\urdf\yaskawaGP12.urdf");
}private void button1_Click(object sender, EventArgs e)
{double z = double.Parse(textBox2.Text);MWArray boxSize = (MWNumericArray)new double[] { 0.5, 0.5, 0.25 };MWArray boxPose = (MWNumericArray)new double[] { -1, 0, 0.2 };MWArray box = matlab.GetBox(boxSize, boxPose);MWArray robotPose = new MWNumericArray(6, 2, new double[] { 3, 3, 1, 1, 0.8, z, 0, 0,             0, 0, Math.PI, Math.PI });MWArray result = matlab.RobotCollisionCheck(robot, box, robotPose);textBox1.Text = result.ToString();
}

相关资源下载：【免费】机械手碰撞检测相关资料资源-CSDN文库