Solidworks 2016中导出URDF文件

安装SolidWorks to URDF exporter插件

　　下载SolidWorks to URDF Exporter插件后按照网站上的步骤进行安装（目前该插件已经在Win 7 64位系统+Solidworks 2017上验证过。So far this add-in has been tested only on Windows 7 64bit with SolidWorks 2017 64bit. It currently does not install on 32bit machines. The add-in makes use of the most up-to-date SW API so I do not expect this tool to work with versions before SolidWorks 2012）。打开Solidworks 2016的插件对话框，可以看到SW2URDF插件已经加载并启动：

导出单个零件到URDF文件

 　　在Solidworks中定义好零件的材质等参数后，在工具→File→Export as URDF中打开导出对话框：

 　　可以打开“工具→评估→质量属性”对话框查看零件质量属性，URDF导出界面中的质量属性参数与Solidworks中的一致，当然也可以在导出时进行手动修改。还可以在Texture栏选择材质贴图文件（You can add a texture to the link by selecting the texture from your computer. This file will be copied into your URDF package），点击Finish后将会在指定的保存目录下生成包含URDF文件、网格文件、材质文件等的一个package.(Click finish to create the package. It will export the meshes, copy the textures and create a URDF that specifies the locations of these files.)

 　　生成的URDF文件（Unified Robot Description Format，是一种特殊的xml 文件格式）如下：

<robot
  name="SarrusLink.SLDPRT">
  <link
    name="SarrusLink">
    <inertial>
      <origin
        xyz="-7.5253E-05 6.87439999999998E-20 -3.9011E-18"
        rpy="0 0 0" />
      <mass
        value="1.9715" />
      <inertia
        ixx="0.00294419345246927"
        ixy="-6.68772298538631E-19"
        ixz="-5.52225503571719E-19"
        iyy="0.0245212201140141"
        iyz="-3.65385697808336E-19"
        izz="0.0217833254619279" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="1.5707963267949 0 0" />
      <geometry>
        <mesh
          filename="package://SarrusLink/meshes/SarrusLink.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.52941 0.54902 0.54902 1" />
        <texture
          filename="package://SarrusLink/textures/timg.jpg" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="1.5707963267949 0 0" />
      <geometry>
        <mesh
          filename="package://SarrusLink/meshes/SarrusLink.STL" />
      </geometry>
    </collision>
  </link>
</robot>

 　　Solidworks中的零件被当做一个link，link可以指定许多属性：

• <inertial>     定义惯性参数（Kinematic and dynamic description of the robot）
• <visual>      定义外观，大小，颜色和材质纹理贴图（Visual representation of the robot）
• <collision>   定义碰撞检测属性（Collision model of the robot）

 V-rep中导入URDF文件 

　　在VREP机器人仿真软件中，可以用URDF import插件导入URDF文件：

 　　选择导入零件的视觉部件（visual part），在Visual properties下的Texture/geometry properties中点击按钮，在弹出的对话框中选择材质贴图导入：

 rviz中显示单个零件 

 　　将Solidworks中的单个零件导出成urdf文件后，为了在rviz中正确显示，可以使用static_transform_publisher发布base_link和固定坐标系map之间的静态变换，以确定零件的位置和姿态。或者可以自己写程序发布tf变换，实时更改零件的位姿。

　　在Solidworks中画一个简单的立方体零件，然后使用SW2URDF插件导出。在ROS工作空间中使用catkin_create_pkg创建一个新的slider包，并创建scripts文件夹用于存放Python程序。

　　scripts文件夹中的slider.py程序（参考joint_state_publisher）：

#!/usr/bin/env python

import rospy
import tf

import wx 
from math import pi
from threading import Thread

class Slider(wx.Frame): 

    def __init__(self, parent, title): 
        super(Slider, self).__init__(parent, title = title,size = (400,300))  
        self.InitUI() 
        self.roll = 0.0
        self.pitch = 0.0
        self.yaw = 0.0

    def InitUI(self):    
        panel = wx.Panel(self) 
        vbox = wx.BoxSizer(wx.VERTICAL) 

        self.slider1 = wx.Slider(parent = panel,id = 1, value = 0, minValue = -900, maxValue = 900, style= wx.SL_AUTOTICKS | wx.SL_HORIZONTAL) 
        self.slider2 = wx.Slider(parent = panel,id = 2, value = 0, minValue = -900, maxValue = 900, style= wx.SL_AUTOTICKS | wx.SL_HORIZONTAL) 
        self.slider3 = wx.Slider(parent = panel,id = 3, value = 0, minValue = -900, maxValue = 900, style= wx.SL_AUTOTICKS | wx.SL_HORIZONTAL) 
        self.button1 = wx.Button(panel, -1, "Center")     

        vbox.Add(self.slider1,1,flag = wx.EXPAND | wx.ALIGN_CENTER_HORIZONTAL | wx.TOP, border = 20) 
        vbox.Add(self.slider2,1,flag = wx.EXPAND | wx.ALIGN_CENTER_HORIZONTAL | wx.TOP, border = 20) 
        vbox.Add(self.slider3,1,flag = wx.EXPAND | wx.ALIGN_CENTER_HORIZONTAL | wx.TOP, border = 20) 
        vbox.Add(self.button1,1,flag = wx.EXPAND | wx.ALIGN_CENTER_HORIZONTAL | wx.TOP, border = 20) 

        self.slider1.Bind(wx.EVT_SLIDER, self.OnSliderScroll) 
        self.slider2.Bind(wx.EVT_SLIDER, self.OnSliderScroll)    
        self.slider3.Bind(wx.EVT_SLIDER, self.OnSliderScroll) 
        self.button1.Bind(wx.EVT_BUTTON, self.OnClick)  

        panel.SetSizer(vbox) 
        self.Centre() 
        self.Show(True)      

    def OnSliderScroll(self, event): 
        obj = event.GetEventObject() 
        val = obj.GetValue() * pi / 1800.0
        id = event.GetId()
        if id == 1:
            self.roll = val
        elif id == 2:
            self.pitch = val
        elif id ==3 :
            self.yaw = val

        #print self.roll, self.pitch, self.yaw
        


    def OnClick(self, event): 
        self.slider1.SetValue(0)
        self.slider2.SetValue(0)
        self.slider3.SetValue(0)
        self.roll = 0.0
        self.pitch = 0.0
        self.yaw = 0.0




def send_tf():

    ex = wx.App() 
    s = Slider(None,'Change Orientation') 

    rospy.init_node('tf_broadcaster')

    br = tf.TransformBroadcaster()

    rate = rospy.Rate(10) # 10hz

    Thread(target=ex.MainLoop).start()

    while not rospy.is_shutdown():
        br.sendTransform((0,0,0), 
                         tf.transformations.quaternion_from_euler(s.roll,s.pitch,s.yaw), 
                        rospy.Time.now(), 
                        "base_link", 
                        "map")
        rate.sleep()




if __name__ == '__main__':          

    try:
        send_tf()

    except rospy.ROSInterruptException: pass

　　part.urdf文件：

<robot
  name="part">
  <link
    name="base_link">
    <inertial>
      <origin
        xyz="0 0 -0.025"
        rpy="0 0 0" />
      <mass
        value="0.125" />
      <inertia
        ixx="5.20833333333333E-05"
        ixy="0"
        ixz="0"
        iyy="5.20833333333333E-05"
        iyz="0"
        izz="5.20833333333333E-05" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 -0.025"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://slider/meshes/part.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.45294 0.75294 0.35294 1" />
        <texture
          filename="package://slider/textures/timg.jpg" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://slider/meshes/part.STL" />
      </geometry>
    </collision>
  </link>
</robot>

　　display.launch文件：

<launch>
    <param name="robot_description" textfile="$(find slider)/urdf/part.urdf" />

    <node name="slider" pkg="slider" type="slider.py" />

    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find slider)/part.rviz"  required="true" />
</launch>

　　Python中创建GUI界面用到了wxPython库。由于GUI和ROS分别有自己的事件循环，为了不相互阻塞，可以在主线程中执行ROS循环，然后新开启一个线程执行GUI线程。代码中函数对象ex.MainLoop作为参数传给它的初始化函数，再调用Thread对象的start方法，线程启动后将执行ex.MainLoop函数。ROS程序以10Hz的频率发布base_link相对于map的变换关系，这里只改变其姿态（GUI界面上拖动滑块时可以改变RPY角）。

　　threading.Thread类的初始化函数原型：

def __init__(self, group=None, target=None, name=None, args=(), kwargs={})

• 参数group是预留的，用于将来扩展；
• 参数target是一个可调用对象，在线程启动后执行；
• 参数name是线程的名字。默认值为“Thread-N”，N是一个数字。
• 参数args和kwargs分别表示调用target时的参数列表和关键字参数。

　　使用catkin_make编译一下slider包，并注意环境变量的设置。没问题后输入下面命令打开rviz，同时通过GUI界面上的三个滑块调整物体姿态：

$ roslaunch slider display.launch

 导出装配体到URDF文件

 　　SW2URDF插件并没有智能到能自动识别Solidworks装配体中的约束，然后正确导出URDF文件，还需要我们自己在装配体中建立参考坐标系、参考轴以及指定轴的类型。如下图所示Joint axis的方向定义在Joint frame中，而Joint frame则相对于其父坐标系Parent frame定义：

　　下面我们以达芬奇手术机器人的一个臂为例子，将其Solidworks装配体导出成URDF文件并在rviz中显示。首先需要自己建立机器人的关节坐标系和关节参考轴，在装配体界面的参考几何体菜单中我们可以建立基准面、基准轴、坐标系、点等一系列参考几何体：

　　按照通常习惯我们以Z轴作为关节参考轴的方向，即如果是移动关节那么就沿着Z轴方向移动，如果是转动关节则按照右手定则绕Z轴转动。将参考坐标系frame0建立在第一个零件base_link上，frame1建立在link1上...那么整个运动链为：frame0/base_link→ joint1 →frame1/link1→ joint2 →frame2/link2→ joint3 →frame3/link3→ joint4 →frame4/link4

　　创建好参考坐标系和参考轴后打开SW2URDF插件，按顺序正确配置参数：

 　　配置好运动链后点击Preview and Export按钮会弹出一个界面，现在需要配置关节的一些参数。需要注意的是Solidworks中建立的参考轴只有方向而没有正负，比如Axis=(0,0,1)则运动的正方向沿着Joint frame Z轴的正方，而如果是Axis=(0,0,-1)则运动的正方向沿着Joint frame Z轴的负方向，因此要注意正负号。

　　另外还需要注意如果选择prismatic或revolute这种非continuous类型的关节，需要在Limit中设定轴的运动范围，否则后面执行launch文件时会出错。下面就是没有设置关节运动范围的错误提示：

　　可以先看看生成的URDF文件中对关节1的描述，可以知道joint1是移动关节，父对象为base_link，子对象为link1。移动的正方向沿着joint1坐标系（frame1）的Z轴正方向，关节移动上限为5mm，下限为-415mm。origin标签中xyz表示frame1的原点在frame0中的位置，rpy表示frame1相对于frame0的姿态角。

<joint
    name="joint1"
    type="prismatic">
    <origin
      xyz="0.415 0 0.046285"
      rpy="1.5708 9.2354E-16 1.5708" />
    <parent
      link="base_link" />
    <child
      link="link1" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-0.415"
      upper="0.05"
      effort="0"
      velocity="0" />
</joint>

　　在Solidworks中测量可知frame1相对于frame0的位置为(0.415, 0, 0.04629)，用RPY角表示的姿态为(90°, 0, 90°)，可见与URDF文件的描述一致。

　　关节参数配置好后点击Next进入连杆配置界面，可以在里面设置连杆质量、惯性张量、颜色、材质等属性：

 　　配置好后点击Finish，将生成的文件夹保存到指定的目录下。我们指定的文件夹名为test，里面有一些子文件夹：urdf中包含了机器人的urdf文件，textures里面放着材质贴图，meshes中放着模型的STL文件，launch文件夹中有display.launch文件可以在rviz中查看机器人。

　　自动生成的文件中还存在一些问题需要修改。package.xml文件中的email是不合法的，只要改成合法的email就可以了

<maintainer email="me2email.com" />    <!-- 可以改为：me@email.com  -->

　　另外display.launch文件里中gui参数要设为True，否则无法打开Joint State Publish工具

<arg name="gui" default="False" />     <!-- 将False改为True  -->

　　还有查找urdf文件时要将路径中的robots改为urdf，或者直接将urdf文件夹重命名为robots

<param name="robot_description" textfile="$(find test)/robots/test.urdf" />   <!-- robots改为urdf  -->

　　改完后将test文件夹移动到ROS工作目录下的源代码目录中（比如catkin_ws/src下），然后使用catkin工具编译一下。下面执行roslaunch test display.launch，在rviz中添加RobotModel，并将Fixed Frame设为base_link，此时机器人就能正确显示出来。这里需要注意Solidworks中建立的frame0坐标系的朝向会影响机器人在rviz中的初始姿态。另外，如果还想观察关节运动时坐标系的变化，可以点击Add按钮添加TF。

　　拖动Joint State Publisher界面上的滑块可以改变对应关节的位置，最左边是之前设定的lower limit，最右边是upper limit. 

 

参考：

http://wiki.ros.org/sw_urdf_exporter

http://wiki.ros.org/sw_urdf_exporter/Tutorials

XML Robot Description Format (URDF)

rviz使用参考

WxPython教程

ROS专题----机器人模型urdf简明笔记

Python模块学习：threading 多线程控制和处理

Solidworks模型转化为URDF文件格式+三连杆机械臂示例+逆运动学