kuka协作机器人LBR系列 issy15R930导入到ros2_rviz(带外观文件)外观文件未调整好,外观仍需进一步研究,外观文件dae与轮廓(碰撞)文件STL并未完全对应起来。在blender里面看了一下UR机器人的文件,是对应的,本人导出的两种文件总是存在各中角度差。需要进一步研究。
本文参考了以下两篇文章,和UR的包,本文未详细论述的地方,也可以按参考文章中的相关章节进行操作。
【ROS2机器人入门到实战】RVIZ2可视化移动机器人模型_rviz移动模型-CSDN博客
Solidworks模型转换到URDF格式并配置Moveit的详细教程_sw_urdf_exporter将soildwork的模型导为urdf文件怎么安装-CSDN博客 如何下载kuka机器人模型,可以在xpert上注册账号下载,也可以联系本人帮忙下载。
1.solidworks安装sw2urdf插件
Releases · ros/solidworks_urdf_exporter · GitHub
我用的solidworks是2023版SP5,经过验证,该sw2urdf插件是完全可以使用的。
安装好后,还不太好找在哪里,具体位置如上图
2.solidworks安装导出dae文件的插件
Simlab 3D Plugins - Collada exporter for SolidWorks
这个插件要收费,不过可以试用一个月。安装好后如下图
3.建立机器人模型的轴和坐标系
在建立坐标系的时候可以先隐藏相关的组件,按下图建立六个轴的基准轴和坐标系,底座也要建立一个坐标系。
可以使用两点生成轴,也可以通过一个圆心点和那个圆面生成。效果都差不多。我都是建在凹坑里面,建到凸台上也是可以的。 建好后如下图
4.sw2urdf插件配置
这些个都要写,不要搞成自动生成,差别太大,不行的。
load configuration..是指你导出后,在urdf文件中有个CSV文件,可以选用上次的配置。因为每次进来都是自动生成轴,想修改很麻烦,每次都点击这么多。
点击Preview and export..按钮,导出前进行确认和配置,关键类型选择有限旋转的这个。如果不配置角度的话,rviz中轴操作无法进行。怎么配置这个角度,可以参考下一节。
这里面的颜色就保持默认就好,好像改了也没有太大效果。点击导出urdf和meshes文件即可。
导出后大概就是这样的,如下图。
5.轴关节限值配置
在kuka xpert上下载MA_LBR_iisy_zh.pdf这个文件,不会下载的可以找我要。在目录里面找到issy15R930轴数据。
把这个相关的角度计算成弧度,第一个185就是185/360*3.14=1.61,其他的类似。
第四步导出模型的时候要跟这个图片的零点位置差不多,这些角度就是上图的姿态为零点的。
6.建立ros2编译文件工程
6.1建pyhton格式的文件工程
ros2 pkg create issy15r930 --build-type ament_python
在生成的工程目录里面复制meshes和urdf文件从第四步得到的那个文件,launch文件夹是自己建的,可以用命令,也可以界面上直接操作,怎么方便怎么来。
6.2 lauch文件说明
就搞了这么一个launch文件,里面比较重要的就是urdf_name这一条,主要要跟urdf中的对应。
import os
from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageSharedef generate_launch_description():package_name = 'issy15r930'urdf_name = "issy15r930.urdf"ld = LaunchDescription()pkg_share = FindPackageShare(package=package_name).find(package_name) urdf_model_path = os.path.join(pkg_share, f'urdf/{urdf_name}')robot_state_publisher_node = Node(package='robot_state_publisher',executable='robot_state_publisher',arguments=[urdf_model_path])joint_state_publisher_node = Node(package='joint_state_publisher_gui',executable='joint_state_publisher_gui',name='joint_state_publisher_gui',arguments=[urdf_model_path])rviz2_node = Node(package='rviz2',executable='rviz2',name='rviz2',output='screen',)ld.add_action(robot_state_publisher_node)ld.add_action(joint_state_publisher_node)ld.add_action(rviz2_node)return ld
6.3 meshes文件说明
dae文件怎么导出,见第7节说明,和STL放一个文件夹就行。不用太费事。主要是urdf文件中要写对文件地址就好。
6.4 urdf文件说明
这个名字要和launch文件里面对应。
<?xml version="1.0" encoding="utf-8"?><!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com) Commit Version: 1.6.0-4-g7f85cfe Build Version: 1.6.7995.38578For more information, please see http://wiki.ros.org/sw_urdf_exporter --><robotname="issy15r930"><linkname="base_link"><inertial><originxyz="3.7076E-05 -0.018098 0.082416"rpy="0 0 0" /><massvalue="7.2283" /><inertiaixx="0.052936"ixy="2.5994E-05"ixz="-5.0195E-05"iyy="0.040376"iyz="0.0027943"izz="0.051987" /></inertial><visual><originxyz="0 0 0"rpy="0 0 0" /><geometry><meshfilename="package://issy15r930/meshes/base_link.dae" /></geometry><materialname=""><colorrgba="0.79216 0.81961 0.93333 1" /></material></visual><collision><originxyz="0 0 0"rpy="0 0 0" /><geometry><meshfilename="package://issy15r930/meshes/base_link.STL" /></geometry></collision></link><linkname="link1"><inertial><originxyz="0.0082771 -0.010407 0.10376"rpy="0 0 0" /><massvalue="6.0436" /><inertiaixx="0.034011"ixy="0.00069966"ixz="-0.0020617"iyy="0.033685"iyz="0.0026113"izz="0.030166" /></inertial><visual><originxyz="0 0 0"rpy="0 0 0" /><geometry><meshfilename="package://issy15r930/meshes/link1.dae" /></geometry><materialname=""><colorrgba="0.79216 0.81961 0.93333 1" /></material></visual><collision><originxyz="0 0 0"rpy="0 0 0" /><geometry><meshfilename="package://issy15r930/meshes/link1.STL" /></geometry></collision></link><jointname="joint1"type="revolute"><originxyz="0 0 0.172"rpy="0 0 0" /><parentlink="base_link" /><childlink="link1" /><axisxyz="0 0 1" /><limitlower="-1.61"upper="1.61"effort="0"velocity="0" /></joint><linkname="link2"><inertial><originxyz="-0.035772 0.16784 0.08072"rpy="0 0 0" /><massvalue="10.123" /><inertiaixx="0.26037"ixy="0.048481"ixz="0.0009505"iyy="0.043052"iyz="-0.0044562"izz="0.27629" /></inertial><visual><originxyz="0 0 0"rpy="0 0 0" /><geometry><meshfilename="package://issy15r930/meshes/link2.dae" /></geometry><materialname=""><colorrgba="0.79216 0.81961 0.93333 1" /></material></visual><collision><originxyz="0 0 0"rpy="0 0 0" /><geometry><meshfilename="package://issy15r930/meshes/link2.STL" /></geometry></collision></link><jointname="joint2"type="revolute"><originxyz="0.058615 -0.074125 0.126"rpy="1.5708 0 0.66908" /><parentlink="link1" /><childlink="link2" /><axisxyz="0 0 1" /><limitlower="-2"upper="0.43"effort="0"velocity="0" /></joint><linkname="link3"><inertial><originxyz="0.020571 -0.0055652 0.093536"rpy="0 0 0" /><massvalue="4.3819" /><inertiaixx="0.018095"ixy="0.0010427"ixz="-0.0029257"iyy="0.021705"iyz="0.00078561"izz="0.018661" /></inertial><visual><originxyz="0 0 0"rpy="0 0 0" /><geometry><meshfilename="package://issy15r930/meshes/link3.dae" /></geometry><materialname=""><colorrgba="0.79216 0.81961 0.93333 1" /></material></visual><collision><originxyz="0 0 0"rpy="0 0 0" /><geometry><meshfilename="package://issy15r930/meshes/link3.STL" /></geometry></collision></link><jointname="joint3"type="revolute"><originxyz="-0.080235 0.37655 0.0255"rpy="3.1416 0 0" /><parentlink="link2" /><childlink="link3" /><axisxyz="0 0 1" /><limitlower="-1.3"upper="1.3"effort="0"velocity="0" /></joint><linkname="link4"><inertial><originxyz="-0.0527 0.0075768 0.11883"rpy="0 0 0" /><massvalue="4.3253" /><inertiaixx="0.031922"ixy="0.001207"ixz="0.012121"iyy="0.040091"iyz="-0.0017531"izz="0.019873" /></inertial><visual><originxyz="0 0 0"rpy="0 0 0" /><geometry><meshfilename="package://issy15r930/meshes/link4.dae" /></geometry><materialname=""><colorrgba="0.79216 0.81961 0.93333 1" /></material></visual><collision><originxyz="0 0 0"rpy="0 0 0" /><geometry><meshfilename="package://issy15r930/meshes/link4.STL" /></geometry></collision></link><jointname="joint4"type="revolute"><originxyz="0.10722 -0.028729 0.12"rpy="1.5708 1.2545 1.309" /><parentlink="link3" /><childlink="link4" /><axisxyz="0 0 1" /><limitlower="-1.57"upper="1.57"effort="0"velocity="0" /></joint><linkname="link5"><inertial><originxyz="0.012242 0.00015735 0.07472"rpy="0 0 0" /><massvalue="2.4287" /><inertiaixx="0.0066893"ixy="-1.3173E-05"ixz="-0.00073951"iyy="0.007692"iyz="-9.693E-06"izz="0.0069254" /></inertial><visual><originxyz="0 0 0"rpy="0 0 0" /><geometry><meshfilename="package://issy15r930/meshes/link5.dae" /></geometry><materialname=""><colorrgba="0.79216 0.81961 0.93333 1" /></material></visual><collision><originxyz="0 0 0"rpy="0 0 0" /><geometry><meshfilename="package://issy15r930/meshes/link5.STL" /></geometry></collision></link><jointname="joint5"type="revolute"><originxyz="-0.086604 0.012492 0.256"rpy="1.5708 -1.4484 1.4275" /><parentlink="link4" /><childlink="link5" /><axisxyz="0 0 1" /><limitlower="-0.95"upper="0.95"effort="0"velocity="0" /></joint><linkname="link6"><inertial><originxyz="0.0099764 0.0026101 0.050237"rpy="0 0 0" /><massvalue="1.366" /><inertiaixx="0.0022647"ixy="-0.00025613"ixz="-0.00021904"iyy="0.0031711"iyz="-5.7372E-05"izz="0.0035946" /></inertial><visual><originxyz="0 0 0"rpy="0 0 0" /><geometry><meshfilename="package://issy15r930/meshes/link6.dae" /></geometry><materialname=""><colorrgba="0.79216 0.81961 0.93333 1" /></material></visual><collision><originxyz="0 0 0"rpy="0 0 0" /><geometry><meshfilename="package://issy15r930/meshes/link6.STL" /></geometry></collision></link><jointname="joint6"type="revolute"><originxyz="0.080493 0.0010481 0.0875"rpy="-1.5708 -1.5554 -1.5578" /><parentlink="link5" /><childlink="link6" /><axisxyz="0 0 1" /><limitlower="-1.91"upper="1.91"effort="0"velocity="0" /></joint></robot>
这个文件里面啊,视觉这块,要跟dae对应meshes文件里面的文件。其他的提示找不到的时候都要检查要对应的。7个link里面都是要修改过来的。
6.5 setup文件说明
增加4跟5中的内容
from setuptools import find_packages, setup
from glob import glob
import ospackage_name = 'issy15r930'setup(name=package_name,version='0.0.0',packages=find_packages(exclude=['test']),data_files=[('share/ament_index/resource_index/packages',['resource/' + package_name]),('share/' + package_name, ['package.xml']),(os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),(os.path.join('share', package_name, 'urdf'), glob('urdf/**')),(os.path.join('share', package_name, 'meshes'), glob('meshes/**')),],install_requires=['setuptools'],zip_safe=True,maintainer='cheni',maintainer_email='cheni@todo.todo',description='TODO: Package description',license='TODO: License declaration',tests_require=['pytest'],entry_points={'console_scripts': [],},
)
7.导出dae文件
需要单独打开某个零件,一个一个导出成dae格式,如下图。这个地方可能就是外观和轮廓模型不能对应一致的问题所在。urdf插件肯定处理了什么,这边不知道怎么对应。
文件导出到urdf导出的meshes文件中即可,一起复制到虚拟机的工程目录中即可。
8.编译调试
用这个命令编译
这个命令去引用
这个命令去加载
效果如下:
这个就是只有外观的模型,没太在一起,完全可以控制来回转。
这个是这个碰撞的模型,这个urdf插件导出来的,是可以的,没有任何问题。
两个一起显示就是这样的,外观文件没有对其碰撞文件
9.问题分析
打开ur机器人的dae文件和STL文件,是可以对应的,在blender里面,这个距离是我拉开的,导入是完全重合的。
对比我导入的dae和stl模型,两者就不太对应。还需要进一步研究。
测试了移动原点也是不行的,通过这个插件搞了一下工件中心的原点对齐,也是不行的。还需要进一步确认原因。
xpert下载一次5元https://item.taobao.com/item.htm?app=chrome&bxsign=scdqor6QWU28XNc9OWfCSkU-CHUI-AJuCDWIGWPTosGsbY2Hq5KZcxkBJDupu_V5lItJnPJnOciY07uys5r9R9Ywfte7ZaUVQ7ACg7y6MYkfdzRmk6S9w1zIPPabV-As4cP&cpp=1&id=769766514957&price=5&shareUniqueId=25609527507&share_crt_v=1&shareurl=true&short_name=h.5FtsLWRmcGNYWrz&sourceType=item,item&sp_abtk=gray_1_code_simpleAndroid2&sp_tk=UUdTcFdsMlhXRlQ=&spm=a2159r.13376460.0.0&suid=9815284f-7724-4861-b99f-05be8c53f351&tbSocialPopKey=shareItem&tk=QGSpWl2XWFT&un=8e550dfe9bde5d682040bd82ddc2e1ea&un_site=0&ut_sk=1.ZQ%20rL9czjnsDANpS6QQfKorD_21646297_1708938056189.Copy.1