RobWork机器人框架（3）：加载机器人的场景文件

上一篇文章中讲解了如何基于源码编译RobWork机器人框架，接下来本文将介绍如何使用编译安装好的RobWork机器人框架，这里以加载XML格式的UR3机器人场景文件（这里RobWork的机器人场景文件你可以认为就是ROS机器人框架中的URDF文件）为例，介绍如何使用RobWork框架进行代码开发。

准备工作做完之后，现在就可以创建工程，编写一个简单的hello_robwork。

CMakeLists.txt

cmake_minimum_required(VERSION 3.5)

project(hello_robwork LANGUAGES CXX)

set(CMAKE_CXX_STANDARD 11)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(RobWork_DIR "/home/qiang/RobWork/RobWork/cmake")
find_package(RobWork)
include_directories(${ROBWORK_INCLUDE_DIRS})
add_executable(hello_robwork main.cpp)
target_link_libraries(hello_robwork ${ROBWORK_LIBRARIES})

main.cpp

#include <iostream>
#include <rw/loaders/WorkCellLoader.hpp>
#include <rw/models/WorkCell.hpp>
#include <rw/common/Ptr.hpp>

using namespace rw::common;
using namespace rw::loaders;
using namespace rw::models;

int main() {
    WorkCell::Ptr workcell =  WorkCellLoader::Factory::load("/home/qiang/UR3_2015/UR3.xml");
    std::cout << workcell->getName() << "\n";
    std::cout << workcell->getFilename() << "\n";
    std::cout << workcell->getFilePath() << "\n";
    return 0;
}

输出的结果为：

/home/qiang/UR3_2015/UR3.xml
/home/qiang/UR3_2015/

我们会发现，第一行的输出为空，这个很好解释，因为我们加载的UR3.xml文件是一个SerialDevice设备文件，并不是一个WorkCell工作单元文件，所以最简单的判别方法就是查看xml文件中是否有WorkCell标签，如果没有就不是WorkCell工作单元文件，如果没有WorkCell标签，但是有SerialDevice标签，那么就说明这是一个SerialDevice设备文件。

UR3.xml

<SerialDevice name="UR3_2015">
	<Frame name="Base" />

	<!-- Include either DHParam.xml or PosRPYParam.xml -->
	<Include file="DHParam.xml" />

	<Frame name="Flange" refframe="Joint5">
		<RPY>0 0 0</RPY>
		<Pos>0 0 0.0799</Pos>
	</Frame>

	<PosLimit refjoint="Joint0" min="-360" max="360" />
	<PosLimit refjoint="Joint1" min="-450" max="270" />
	<PosLimit refjoint="Joint2" min="-360" max="360" />
	<PosLimit refjoint="Joint3" min="-450" max="270" />
	<PosLimit refjoint="Joint4" min="-360" max="360" />
	<PosLimit refjoint="Joint5" min="-1080" max="1080" /> <!-- Unlimited! -->

	<VelLimit refjoint="Joint0" max="180" />
	<VelLimit refjoint="Joint1" max="180" />
	<VelLimit refjoint="Joint2" max="180" />
	<VelLimit refjoint="Joint3" max="360" />
	<VelLimit refjoint="Joint4" max="360" />
	<VelLimit refjoint="Joint5" max="360" />

	<AccLimit refjoint="Joint0" max="120" />
	<AccLimit refjoint="Joint1" max="120" />
	<AccLimit refjoint="Joint2" max="120" />
	<AccLimit refjoint="Joint3" max="120" />
	<AccLimit refjoint="Joint4" max="120" />
	<AccLimit refjoint="Joint5" max="120" />

	<Drawable name="flangeGeo" refframe="Base">
		<RPY>180 0 90</RPY>
		<Pos> 0 0 0</Pos>
		<Polytope file="Geometry/Base" />
	</Drawable>

	<Drawable name="Joint0Geo" refframe="Joint0">
		<RPY>180 0 90</RPY>
		<Pos>0 0 0</Pos>
		<Polytope file="Geometry/Joint0" />
	</Drawable>

	<Drawable name="Joint1Geo" refframe="Joint1">
		<RPY> 90 180 0 </RPY>
		<Pos>0.1519 0 0</Pos>
		<Polytope file="Geometry/Joint1" />
	</Drawable>

	<Drawable name="Joint2Geo" refframe="Joint2">
		<RPY>90 180 0 </RPY>
		<Pos>0.39555 0 0</Pos>
		<Polytope file="Geometry/Joint2" />
	</Drawable>

	<Drawable name="Joint3Geo" refframe="Joint3">
		<RPY> 0 0 180 </RPY>
		<Pos> 0 0.60855 0</Pos>
		<Polytope file="Geometry/Joint3" />
	</Drawable>

	<Drawable name="Joint4Geo" refframe="Joint4">
		<RPY>0 0 90</RPY>
		<Pos>0 -0.1104 -0.60855</Pos>
		<Polytope file="Geometry/Joint4" />
	</Drawable>

	<Drawable name="Joint5Geo" refframe="Flange">
		<RPY>180 -180 0</RPY>
		<Pos>0 0.69195 -0.1903</Pos>
		<Polytope file="Geometry/ToolFlange" />
	</Drawable>

	<ProximitySetup file="ProximitySetup.xml" />

	<Q name="Home">1.57 -2.4 2.4 -1.57 -1.57 0</Q>

</SerialDevice>

DHParam.xml

<DHJoint name="Joint0" alpha="0"   a="0"        d="0"       offset="0" type="schilling" />
<DHJoint name="Joint1" alpha="90"  a="0"        d="0.1519"  offset="0" type="schilling" />
<DHJoint name="Joint2" alpha="0"   a="-0.24365" d="0"       offset="0" type="schilling" />
<DHJoint name="Joint3" alpha="0"   a="-0.213"   d="0"       offset="0" type="schilling" />
<DHJoint name="Joint4" alpha="90"  a="0"        d="0.1104"  offset="0" type="schilling" />
<DHJoint name="Joint5" alpha="-90" a="0"        d="0.0834"  offset="0" type="schilling" />

ProximitySetup.xml

<?xml version="1.0" encoding="UTF-8" standalone="no" ?>
<ProximitySetup UseIncludeAll="true" UseExcludeStaticPairs="true">
    <Exclude PatternA="Base" PatternB="Joint0"/>
	<Exclude PatternA="Joint0" PatternB="Joint1"/>
	<Exclude PatternA="Joint1" PatternB="Joint2"/>
	<Exclude PatternA="Joint2" PatternB="Joint3"/>
	<Exclude PatternA="Joint3" PatternB="Joint4"/>
	<Exclude PatternA="Joint4" PatternB="Flange"/>
</ProximitySetup>