将5DOF机械臂相关代码移动到5DOF_turtlebot_arm分支上，master分支默认不再包含该代码

src/turtlebot_arm/.gitignore

@@ -1 +0,0 @@
-.settings

src/turtlebot_arm/LICENSE

@@ -1,28 +0,0 @@
-BSD 3-Clause License
-
-Copyright (c) 2021, Willow Garage, Inc.
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-
-    * Redistributions of source code must retain the above copyright
-      notice, this list of conditions and the following disclaimer.
-    * Redistributions in binary form must reproduce the above copyright
-      notice, this list of conditions and the following disclaimer in the
-      documentation and/or other materials provided with the distribution.
-    * Neither the name of the Willow Garage, Inc. nor the names of its
-      contributors may be used to endorse or promote products derived from
-      this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-POSSIBILITY OF SUCH DAMAGE.

src/turtlebot_arm/README.md

@@ -1,32 +0,0 @@
-TurtleBot Arm
-=============
-
-## Updated for Kinetic!
-
-Kinetic version of turtlebot arm code. I didn't try on Jade, but it should easily work with minor changes. It's mostly the same code as on Indigo, with some improvements on turtlebot_arm_block_manipulation (see updated wiki <span style="background-color: #00FFFF">TODO</span>) and specially [turtlebot_arm_object_manipulation](https://github.com/turtlebot/turtlebot_arm/tree/kinetic-devel/turtlebot_arm_object_manipulation): a fully revised version of that demo using OKR, SMACH and MoveIt! pick and place.
-
-## Selecting Arm Type
-By default this will work with the original white/green TurtleBot arm.  To use the PhantomX Pincher, set environment variable "TURTLEBOT_ARM1" to pincher. You will need arbotix_ros version 0.11.0 or higher for PhantomX Pincher.
-
-## Attaching the Arm to a Robot
-Open your xacro-macro-magic URDF, and add something like:
-
-       <include filename="$(find turtlebot_arm_description)/urdf/$(optenv TURTLEBOT_ARM1 turtlebot)_arm.xacro" />
-       <turtlebot_arm parent="base_link" color="white" gripper_color="green"
-                 joints_vlimit="1.571" pan_llimit="-2.617" pan_ulimit="2.617">
-          <origin xyz="0 0 1"/>
-       </turtlebot_arm>
-
-This will attach a turtlebot arm to your robot. Replace base_link with whatever link you want to attach to, and change the origin as needed. Apart from color, we can configure joints velocity limit and lower/upper limits for the first joint (arm_shoulder_pan) to allow accessing to different operational areas, e.g. left handed vs. right handed robot
-
-## Running MoveIt
-Ensure you have all the required dependencies by running (you probably did it before compiling):
-
-       cd turtlebot_arm
-       rosdep install --from-paths -i -y src
-
-Before start playing, I highly recommend to calibrate your camera extrinsic parameters following [this tutorial](http://wiki.ros.org/turtlebot_kinect_arm_calibration/Tutorials/CalibratingKinectToTurtleBotArm)
-
-The turtlebot_arm_moveit_config/demo.launch is a bit wonky, but the move_group action underlying it works fine. To test your installation, run one of the demos (well, demo by now) from turtlebot_arm_moveit_demos, e.g.:
-
-       rosrun turtlebot_arm_moveit_demos pick_and_place.py

src/turtlebot_arm/turtlebot_arm/.cproject

@@ -1,43 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" standalone="no"?>
-<?fileVersion 4.0.0?>
-
-<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
-	<storageModule moduleId="org.eclipse.cdt.core.settings">
-		<cconfiguration id="cdt.managedbuild.toolchain.gnu.base.647447099">
-			<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.base.647447099" moduleId="org.eclipse.cdt.core.settings" name="Default">
-				<externalSettings/>
-				<extensions>
-					<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
-					<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
-				</extensions>
-			</storageModule>
-			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
-				<configuration buildProperties="" id="cdt.managedbuild.toolchain.gnu.base.647447099" name="Default" parent="org.eclipse.cdt.build.core.emptycfg">
-					<folderInfo id="cdt.managedbuild.toolchain.gnu.base.647447099.152571619" name="/" resourcePath="">
-						<toolChain id="cdt.managedbuild.toolchain.gnu.base.584652511" name="cdt.managedbuild.toolchain.gnu.base" superClass="cdt.managedbuild.toolchain.gnu.base">
-							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="cdt.managedbuild.target.gnu.platform.base.867259224" name="Debug Platform" osList="linux,hpux,aix,qnx" superClass="cdt.managedbuild.target.gnu.platform.base"/>
-							<builder id="cdt.managedbuild.target.gnu.builder.base.629026303" managedBuildOn="false" name="Gnu Make Builder.Default" superClass="cdt.managedbuild.target.gnu.builder.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.archiver.base.1183691927" name="GCC Archiver" superClass="cdt.managedbuild.tool.gnu.archiver.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.cpp.compiler.base.79254247" name="GCC C++ Compiler" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.c.compiler.base.1809545855" name="GCC C Compiler" superClass="cdt.managedbuild.tool.gnu.c.compiler.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.c.linker.base.1559981847" name="GCC C Linker" superClass="cdt.managedbuild.tool.gnu.c.linker.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.cpp.linker.base.165109707" name="GCC C++ Linker" superClass="cdt.managedbuild.tool.gnu.cpp.linker.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.assembler.base.638281991" name="GCC Assembler" superClass="cdt.managedbuild.tool.gnu.assembler.base"/>
-						</toolChain>
-					</folderInfo>
-				</configuration>
-			</storageModule>
-			<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
-		</cconfiguration>
-	</storageModule>
-	<storageModule moduleId="cdtBuildSystem" version="4.0.0">
-		<project id="turtlebot.null.2019119152" name="turtlebot"/>
-	</storageModule>
-	<storageModule moduleId="scannerConfiguration">
-		<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
-	</storageModule>
-</cproject>

src/turtlebot_arm/turtlebot_arm/.project

@@ -1,79 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<projectDescription>
-	<name>turtlebot_arm</name>
-	<comment></comment>
-	<projects>
-	</projects>
-	<buildSpec>
-		<buildCommand>
-			<name>org.eclipse.cdt.managedbuilder.core.genmakebuilder</name>
-			<triggers>clean,full,incremental,</triggers>
-			<arguments>
-				<dictionary>
-					<key>?name?</key>
-					<value></value>
-				</dictionary>
-				<dictionary>
-					<key>org.eclipse.cdt.make.core.append_environment</key>
-					<value>true</value>
-				</dictionary>
-				<dictionary>
-					<key>org.eclipse.cdt.make.core.autoBuildTarget</key>
-					<value>all</value>
-				</dictionary>
-				<dictionary>
-					<key>org.eclipse.cdt.make.core.buildArguments</key>
-					<value></value>
-				</dictionary>
-				<dictionary>
-					<key>org.eclipse.cdt.make.core.buildCommand</key>
-					<value>make</value>
-				</dictionary>
-				<dictionary>
-					<key>org.eclipse.cdt.make.core.cleanBuildTarget</key>
-					<value>clean</value>
-				</dictionary>
-				<dictionary>
-					<key>org.eclipse.cdt.make.core.contents</key>
-					<value>org.eclipse.cdt.make.core.activeConfigSettings</value>
-				</dictionary>
-				<dictionary>
-					<key>org.eclipse.cdt.make.core.enableAutoBuild</key>
-					<value>false</value>
-				</dictionary>
-				<dictionary>
-					<key>org.eclipse.cdt.make.core.enableCleanBuild</key>
-					<value>true</value>
-				</dictionary>
-				<dictionary>
-					<key>org.eclipse.cdt.make.core.enableFullBuild</key>
-					<value>true</value>
-				</dictionary>
-				<dictionary>
-					<key>org.eclipse.cdt.make.core.fullBuildTarget</key>
-					<value>all</value>
-				</dictionary>
-				<dictionary>
-					<key>org.eclipse.cdt.make.core.stopOnError</key>
-					<value>true</value>
-				</dictionary>
-				<dictionary>
-					<key>org.eclipse.cdt.make.core.useDefaultBuildCmd</key>
-					<value>true</value>
-				</dictionary>
-			</arguments>
-		</buildCommand>
-		<buildCommand>
-			<name>org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder</name>
-			<triggers>full,incremental,</triggers>
-			<arguments>
-			</arguments>
-		</buildCommand>
-	</buildSpec>
-	<natures>
-		<nature>org.eclipse.cdt.core.cnature</nature>
-		<nature>org.eclipse.cdt.core.ccnature</nature>
-		<nature>org.eclipse.cdt.managedbuilder.core.managedBuildNature</nature>
-		<nature>org.eclipse.cdt.managedbuilder.core.ScannerConfigNature</nature>
-	</natures>
-</projectDescription>

src/turtlebot_arm/turtlebot_arm/CHANGELOG.rst

@@ -1,18 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package turtlebot_arm
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-0.3.3 (2014-09-20)
-------------------
-* Add turtlebot_arm_block_manipulation package
-
-0.3.2 (2014-08-30)
-------------------
-
-0.3.1 (2014-08-22)
-------------------
-* Add a metapackage
-
-0.3.0 (2014-08-22)
-------------------
-* First indigo release

src/turtlebot_arm/turtlebot_arm/CMakeLists.txt

@@ -1,4 +0,0 @@
-cmake_minimum_required(VERSION 2.8.3)
-project(turtlebot_arm)
-find_package(catkin REQUIRED)
-catkin_metapackage()

src/turtlebot_arm/turtlebot_arm/package.xml

@@ -1,28 +0,0 @@
-<package>
-  <name>turtlebot_arm</name>
-  <version>0.3.3</version>
-  <description>
-    The turtlebot arm meta package.
-  </description>
-  <author>Jorge Santos</author>
-  <maintainer email="jsantossimon@gmail.com">Jorge Santos</maintainer>
-  <license>BSD</license>
-  <url type="website">http://ros.org/wiki/turtlebot_arm</url>
-  <url type="repository">https://github.com/turtlebot/turtlebot_arm</url>
-  <url type="bugtracker">https://github.com/turtlebot/turtlebot_arm/issues</url>
-
-  <buildtool_depend>catkin</buildtool_depend>
-
-  <run_depend>turtlebot_arm_bringup</run_depend>
-  <run_depend>turtlebot_arm_description</run_depend>
-  <run_depend>turtlebot_arm_ikfast_plugin</run_depend>
-  <run_depend>turtlebot_arm_block_manipulation</run_depend>
-  <run_depend>turtlebot_arm_object_manipulation</run_depend>
-  <run_depend>turtlebot_arm_kinect_calibration</run_depend>
-  <run_depend>turtlebot_arm_moveit_config</run_depend>
-  <run_depend>turtlebot_arm_moveit_demos</run_depend>
-
-  <export>
-    <metapackage/>
-  </export>
-</package>

src/turtlebot_arm/turtlebot_arm_block_manipulation/.cproject

@@ -1,42 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" standalone="no"?>
-<?fileVersion 4.0.0?><cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
-	<storageModule moduleId="org.eclipse.cdt.core.settings">
-		<cconfiguration id="cdt.managedbuild.toolchain.gnu.base.1352411691">
-			<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.base.1352411691" moduleId="org.eclipse.cdt.core.settings" name="Default">
-				<externalSettings/>
-				<extensions>
-					<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
-					<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
-				</extensions>
-			</storageModule>
-			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
-				<configuration buildProperties="" id="cdt.managedbuild.toolchain.gnu.base.1352411691" name="Default" parent="org.eclipse.cdt.build.core.emptycfg">
-					<folderInfo id="cdt.managedbuild.toolchain.gnu.base.1352411691.852868461" name="/" resourcePath="">
-						<toolChain id="cdt.managedbuild.toolchain.gnu.base.330484760" name="Linux GCC" superClass="cdt.managedbuild.toolchain.gnu.base">
-							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="cdt.managedbuild.target.gnu.platform.base.351303327" name="Debug Platform" osList="linux,hpux,aix,qnx" superClass="cdt.managedbuild.target.gnu.platform.base"/>
-							<builder id="cdt.managedbuild.target.gnu.builder.base.559412359" managedBuildOn="false" name="Gnu Make Builder.Default" superClass="cdt.managedbuild.target.gnu.builder.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.archiver.base.432255193" name="GCC Archiver" superClass="cdt.managedbuild.tool.gnu.archiver.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.cpp.compiler.base.1984677268" name="GCC C++ Compiler" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.c.compiler.base.920409071" name="GCC C Compiler" superClass="cdt.managedbuild.tool.gnu.c.compiler.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.c.linker.base.310953426" name="GCC C Linker" superClass="cdt.managedbuild.tool.gnu.c.linker.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.cpp.linker.base.1340228691" name="GCC C++ Linker" superClass="cdt.managedbuild.tool.gnu.cpp.linker.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.assembler.base.854557860" name="GCC Assembler" superClass="cdt.managedbuild.tool.gnu.assembler.base"/>
-						</toolChain>
-					</folderInfo>
-				</configuration>
-			</storageModule>
-			<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
-		</cconfiguration>
-	</storageModule>
-	<storageModule moduleId="cdtBuildSystem" version="4.0.0">
-		<project id="turtlebot_arm_block_manipulation.null.581804386" name="turtlebot_arm_block_manipulation"/>
-	</storageModule>
-	<storageModule moduleId="scannerConfiguration">
-		<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
-	</storageModule>
-	<storageModule moduleId="org.eclipse.cdt.core.LanguageSettingsProviders"/>
-</cproject>

src/turtlebot_arm/turtlebot_arm_block_manipulation/.project

@@ -1,27 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<projectDescription>
-	<name>turtlebot_arm_block_manipulation</name>
-	<comment></comment>
-	<projects>
-	</projects>
-	<buildSpec>
-		<buildCommand>
-			<name>org.eclipse.cdt.managedbuilder.core.genmakebuilder</name>
-			<triggers>clean,full,incremental,</triggers>
-			<arguments>
-			</arguments>
-		</buildCommand>
-		<buildCommand>
-			<name>org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder</name>
-			<triggers>full,incremental,</triggers>
-			<arguments>
-			</arguments>
-		</buildCommand>
-	</buildSpec>
-	<natures>
-		<nature>org.eclipse.cdt.core.cnature</nature>
-		<nature>org.eclipse.cdt.core.ccnature</nature>
-		<nature>org.eclipse.cdt.managedbuilder.core.managedBuildNature</nature>
-		<nature>org.eclipse.cdt.managedbuilder.core.ScannerConfigNature</nature>
-	</natures>
-</projectDescription>

src/turtlebot_arm/turtlebot_arm_block_manipulation/CHANGELOG.rst

@@ -1,28 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package turtlebot_arm_block_manipulation
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-0.3.3 (2014-09-20)
-------------------
-* Updated version for indigo/moveit of the old turtlebot_block_manipulation
-
-Forthcoming
------------
-* Add install rules
-* Comment properly and other minor tweaks to improve pickup
-* Allow user restarting, for the case that the block detection fails
-* Reorganize and rename demo files to work with moveit
-* Fully working and cleaned.
-* Ready to try!
-* Addapting turtlebot_arm_block_manipulation package to work with moveit
-  (still a lot to do)
-* Contributors: corot
-
-0.3.2 (2014-08-30)
-------------------
-
-0.3.1 (2014-08-22)
-------------------
-
-0.3.0 (2014-08-17)
-------------------

src/turtlebot_arm/turtlebot_arm_block_manipulation/CMakeLists.txt

@@ -1,69 +0,0 @@
-cmake_minimum_required(VERSION 2.8.3)
-project(turtlebot_arm_block_manipulation)
-
-add_definitions(-std=c++11)
-
-# setup
-find_package(catkin REQUIRED actionlib actionlib_msgs interactive_markers pcl_ros roscpp visualization_msgs moveit_core moveit_ros_planning_interface)
-find_package(Boost REQUIRED system filesystem)
-find_package(PCL REQUIRED)
-link_directories(${catkin_LIBRARY_DIRS} ${Boost_LIBRARY_DIRS} ${PCL_LIBRARY_DIRS})
-include_directories(include
-                    SYSTEM
-                    ${Boost_INCLUDE_DIRS}
-                    ${catkin_INCLUDE_DIRS}
-                    ${PCL_INCLUDE_DIRS}
-                   )
-
-add_action_files(FILES
-                 BlockDetection.action
-                 InteractiveBlockManipulation.action
-                 PickAndPlace.action
-                 )
-
-generate_messages(DEPENDENCIES actionlib_msgs geometry_msgs)
-catkin_package(DEPENDS actionlib actionlib_msgs interactive_markers pcl_ros roscpp visualization_msgs moveit_core moveit_ros_planning_interface)
-
-
-# nodes
-add_executable(block_detection_action_server src/block_detection_action_server.cpp)
-target_link_libraries(block_detection_action_server ${catkin_LIBRARIES} ${PCL_LIBRARIES})
-add_dependencies(block_detection_action_server turtlebot_arm_block_manipulation_gencpp)
-
-add_executable(interactive_manipulation_action_server src/interactive_manipulation_action_server.cpp)
-target_link_libraries(interactive_manipulation_action_server ${catkin_LIBRARIES} ${PCL_LIBRARIES})
-add_dependencies(interactive_manipulation_action_server turtlebot_arm_block_manipulation_gencpp)
-
-add_executable(pick_and_place_action_server src/pick_and_place_action_server.cpp)
-target_link_libraries(pick_and_place_action_server ${catkin_LIBRARIES} ${PCL_LIBRARIES})
-add_dependencies(pick_and_place_action_server turtlebot_arm_block_manipulation_gencpp)
-
-# TODO need review
-# rosbuild_add_executable(block_manipulation test/block_manipulation.cpp)
-# rosbuild_add_executable(block_manipulation_actions test/block_manipulation_actions.cpp)
-
-
-# demo
-add_executable(block_manipulation_demo demo/block_manipulation_demo.cpp)
-target_link_libraries(block_manipulation_demo ${catkin_LIBRARIES})
-
-add_dependencies(block_manipulation_demo turtlebot_arm_block_manipulation_gencpp)
-
-
-# install
-
-install(TARGETS block_detection_action_server interactive_manipulation_action_server pick_and_place_action_server
-        DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
-install(TARGETS block_manipulation_demo
-        DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
-install(DIRECTORY demo
-        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-
-install(DIRECTORY launch
-        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)

src/turtlebot_arm/turtlebot_arm_block_manipulation/action/BlockDetection.action

@@ -1,9 +0,0 @@
-#goal definition
-string frame
-float32 table_height
-float32 block_size
----
-#result definition
-geometry_msgs/PoseArray blocks
----
-#feedback

src/turtlebot_arm/turtlebot_arm_block_manipulation/action/InteractiveBlockManipulation.action

@@ -1,9 +0,0 @@
-#goal definition
-string frame
-float32 block_size
----
-#result definition
-geometry_msgs/Pose pickup_pose
-geometry_msgs/Pose place_pose
----
-#feedback

src/turtlebot_arm/turtlebot_arm_block_manipulation/action/PickAndPlace.action

@@ -1,12 +0,0 @@
-#goal definition
-string frame
-float32 z_up
-float32 gripper_open
-float32 gripper_closed
-geometry_msgs/Pose pickup_pose
-geometry_msgs/Pose place_pose
-string topic
----
-#result definition
----
-#feedback

src/turtlebot_arm/turtlebot_arm_block_manipulation/demo/block_manipulation_demo.cpp

@@ -1,186 +0,0 @@
-/*
- * Copyright (c) 2011, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- *     * Redistributions of source code must retain the above copyright
- *       notice, this list of conditions and the following disclaimer.
- *     * Redistributions in binary form must reproduce the above copyright
- *       notice, this list of conditions and the following disclaimer in the
- *       documentation and/or other materials provided with the distribution.
- *     * Neither the name of the Willow Garage, Inc. nor the names of its
- *       contributors may be used to endorse or promote products derived from
- *       this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#include <ros/ros.h>
-
-#include <actionlib/client/simple_action_client.h>
-#include <turtlebot_arm_block_manipulation/BlockDetectionAction.h>
-#include <turtlebot_arm_block_manipulation/PickAndPlaceAction.h>
-#include <turtlebot_arm_block_manipulation/InteractiveBlockManipulationAction.h>
-
-#include <string>
-#include <sstream>
-
-
-const std::string pick_and_place_topic = "/pick_and_place";
-
-namespace turtlebot_arm_block_manipulation
-{
-
-class BlockManipulationAction
-{
-private:
-  ros::NodeHandle nh_;
-  
-  // Actions and services
-  actionlib::SimpleActionClient<BlockDetectionAction> block_detection_action_;
-  actionlib::SimpleActionClient<InteractiveBlockManipulationAction> interactive_manipulation_action_;
-  actionlib::SimpleActionClient<PickAndPlaceAction> pick_and_place_action_;
-  
-  BlockDetectionGoal block_detection_goal_;
-  InteractiveBlockManipulationGoal interactive_manipulation_goal_;
-  PickAndPlaceGoal pick_and_place_goal_;
-
-  // Parameters
-  std::string arm_link;
-  double gripper_open;
-  double gripper_closed;
-  double z_up;
-  double z_down;
-  double block_size;
-  bool once;
-  
-public:
-
-  BlockManipulationAction() : nh_("~"),
-    block_detection_action_("block_detection", true),
-    interactive_manipulation_action_("interactive_manipulation", true),
-    pick_and_place_action_("pick_and_place", true)
-  {
-    // Load parameters
-    nh_.param<std::string>("arm_link", arm_link, "/arm_link");
-    nh_.param<double>("gripper_open", gripper_open, 0.042);
-    nh_.param<double>("gripper_closed", gripper_closed, 0.024);
-    nh_.param<double>("z_up", z_up, 0.12);
-    nh_.param<double>("table_height", z_down, 0.01);
-    nh_.param<double>("block_size", block_size, 0.03);
-    
-    nh_.param<bool>("once", once, false);
-
-    // Initialize goals
-    block_detection_goal_.frame = arm_link;
-    block_detection_goal_.table_height = z_down;
-    block_detection_goal_.block_size = block_size;
-    
-    pick_and_place_goal_.frame = arm_link;
-    pick_and_place_goal_.z_up = z_up;
-    pick_and_place_goal_.gripper_open = gripper_open;
-    pick_and_place_goal_.gripper_closed = gripper_closed;
-    pick_and_place_goal_.topic = pick_and_place_topic;
-    
-    interactive_manipulation_goal_.block_size = block_size;
-    interactive_manipulation_goal_.frame = arm_link;
-    
-    ROS_INFO("Finished initializing, waiting for servers...");
-    
-    block_detection_action_.waitForServer();
-    ROS_INFO("Found block detection server.");
-    
-    interactive_manipulation_action_.waitForServer();
-    ROS_INFO("Found interactive manipulation.");
-    
-    pick_and_place_action_.waitForServer();
-    ROS_INFO("Found pick and place server.");
-    
-    ROS_INFO("Found servers.");
-  }
-  
-  void detectBlocks()
-  {
-    block_detection_action_.sendGoal(block_detection_goal_, boost::bind( &BlockManipulationAction::addBlocks, this, _1, _2));
-  }
-  
-  void addBlocks(const actionlib::SimpleClientGoalState& state, const BlockDetectionResultConstPtr& result)
-  {
-    ROS_INFO("Got block detection callback. Adding blocks.");
-    geometry_msgs::Pose block;
-    
-    if (state == actionlib::SimpleClientGoalState::SUCCEEDED)
-      ROS_INFO("Succeeded!");
-    else
-    {
-      ROS_INFO("Did not succeed! %s",  state.toString().c_str());
-      ros::shutdown();
-    }
-    interactive_manipulation_action_.sendGoal(interactive_manipulation_goal_, boost::bind( &BlockManipulationAction::pickAndPlace, this, _1, _2));
-  }
-  
-  void pickAndPlace(const actionlib::SimpleClientGoalState& state, const InteractiveBlockManipulationResultConstPtr& result)
-  {
-    ROS_INFO("Got interactive marker callback. Picking and placing.");
-    
-    if (state == actionlib::SimpleClientGoalState::SUCCEEDED)
-      ROS_INFO("Succeeded!");
-    else
-    {
-      ROS_INFO("Did not succeed! %s",  state.toString().c_str());
-      ros::shutdown();
-    }
-    pick_and_place_action_.sendGoal(pick_and_place_goal_, boost::bind( &BlockManipulationAction::finish, this, _1, _2));
-  }
-  
-  void finish(const actionlib::SimpleClientGoalState& state, const PickAndPlaceResultConstPtr& result)
-  {
-    ROS_INFO("Got pick and place callback. Finished!");
-    if (state == actionlib::SimpleClientGoalState::SUCCEEDED)
-      ROS_INFO("Succeeded!");
-    else
-      ROS_INFO("Did not succeed! %s",  state.toString().c_str());
-
-    if (once)
-      ros::shutdown();
-    else
-      detectBlocks();
-  }
-};
-
-};
-
-int main(int argc, char** argv)
-{
-  // initialize node
-  ros::init(argc, argv, "block_manipulation");
-
-  turtlebot_arm_block_manipulation::BlockManipulationAction manip;
-
-  // Setup an asynchronous spinner as the move groups operations need continuous spinning
-  ros::AsyncSpinner spinner(4);
-  spinner.start();
-
-  while (ros::ok())
-  {
-    manip.detectBlocks();
-
-    // Allow user restarting, for the case that the block detection fails
-    std::cout << "Press Enter for restarting block detection" << std::endl;
-    std::cin.ignore();
-  }
-
-  spinner.stop();
-}

src/turtlebot_arm/turtlebot_arm_block_manipulation/demo/block_manipulation_demo.launch

@@ -1,35 +0,0 @@
-<launch>
-  <arg name="publish_camera_tf" default="true"/>
-
-  <include file="$(find turtlebot_bringup)/launch/3dsensor.launch">
-    <arg name="camera"                          value="camera"/>
-    <arg name="3d_sensor"                       value="$(env TURTLEBOT_3D_SENSOR)"/>   <!-- kinect, asus_xtion_pro -->
-    <arg name="publish_tf"                      value="$(arg publish_camera_tf)"/>
-    <!-- We only need pointclouds to detect the blocks -->
-    <arg name="depth_registration"              value="true"/>
-    <arg name="rgb_processing"                  value="true"/>
-    <arg name="ir_processing"                   value="false"/>
-    <arg name="depth_processing"                value="true"/>
-    <arg name="depth_registered_processing"     value="true"/>
-    <arg name="disparity_processing"            value="false"/>
-    <arg name="disparity_registered_processing" value="false"/>
-    <arg name="scan_processing"                 value="false"/>
-  </include>
-
-  <!-- If we are not running the full robot, provide an arbitrary link between arm and 3D camera -->
-  <node if="$(arg publish_camera_tf)" pkg="tf" type="static_transform_publisher" name="link_arm_and_camera"
-                                     args="-0.211 -0.051 0.432 -0.000 0.712 0.020 /arm_base_link /camera_link 100"/>
-
-  <include file="$(find turtlebot_arm_block_manipulation)/launch/block_manipulation.launch" />
-
-  <node name="block_manipulation_demo" pkg="turtlebot_arm_block_manipulation" type="block_manipulation_demo" output="screen" >
-    <param name="arm_link" value="/arm_base_link" />
-    <param name="gripper_open" value="0.05" />
-    <param name="gripper_closed" value="0.022" />
-    <param name="z_up" value="0.05" />
-    <param name="table_height" value="-0.03" />
-    <param name="block_size" value="0.025" />
-  </node>
-
-  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find turtlebot_arm_block_manipulation)/demo/block_manipulation_demo.rviz" />
-</launch>

src/turtlebot_arm/turtlebot_arm_block_manipulation/demo/block_manipulation_demo.rviz

@@ -1,556 +0,0 @@
-Panels:
-  - Class: rviz/Displays
-    Help Height: 84
-    Name: Displays
-    Property Tree Widget:
-      Expanded:
-        - /MotionPlanning1/Planned Path1
-        - /DepthCloud1/Auto Size1
-        - /TF1/Frames1
-      Splitter Ratio: 0.40146
-    Tree Height: 320
-  - Class: rviz/Help
-    Name: Help
-  - Class: rviz/Views
-    Expanded:
-      - /Current View1
-    Name: Views
-    Splitter Ratio: 0.5
-Visualization Manager:
-  Class: ""
-  Displays:
-    - Alpha: 0.5
-      Cell Size: 1
-      Class: rviz/Grid
-      Color: 160; 160; 164
-      Enabled: true
-      Line Style:
-        Line Width: 0.03
-        Value: Lines
-      Name: Grid
-      Normal Cell Count: 0
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Plane: XY
-      Plane Cell Count: 10
-      Reference Frame: <Fixed Frame>
-      Value: true
-    - Class: moveit_rviz_plugin/MotionPlanning
-      Enabled: true
-      Move Group Namespace: ""
-      MoveIt_Goal_Tolerance: 0
-      MoveIt_Planning_Attempts: 10
-      MoveIt_Planning_Time: 5
-      MoveIt_Use_Constraint_Aware_IK: true
-      MoveIt_Warehouse_Host: 127.0.0.1
-      MoveIt_Warehouse_Port: 33829
-      Name: MotionPlanning
-      Planned Path:
-        Links:
-          All Links Enabled: true
-          Expand Joint Details: false
-          Expand Link Details: false
-          Expand Tree: false
-          Link Tree Style: Links in Alphabetic Order
-          arm_base_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          arm_elbow_F10_0_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          arm_elbow_F10_1_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          arm_elbow_F10_2_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          arm_elbow_F3_0_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          arm_elbow_flex_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          arm_elbow_flex_servo_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          arm_shoulder_F10_0_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          arm_shoulder_F10_1_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          arm_shoulder_F10_2_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          arm_shoulder_F3_0_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          arm_shoulder_lift_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          arm_shoulder_lift_servo_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          arm_shoulder_pan_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          arm_shoulder_pan_servo_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          arm_wrist_F3_0_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          arm_wrist_flex_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          arm_wrist_flex_servo_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          base_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          gripper_active_finger_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          gripper_active_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          gripper_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          gripper_servo_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          gripper_static_finger_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          gripper_static_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-        Loop Animation: false
-        Robot Alpha: 0.5
-        Show Robot Collision: false
-        Show Robot Visual: true
-        Show Trail: false
-        State Display Time: 0.05 s
-        Trajectory Topic: /move_group/display_planned_path
-      Planning Metrics:
-        Payload: 1
-        Show Joint Torques: false
-        Show Manipulability: false
-        Show Manipulability Index: false
-        Show Weight Limit: false
-        TextHeight: 0.08
-      Planning Request:
-        Colliding Link Color: 255; 0; 0
-        Goal State Alpha: 1
-        Goal State Color: 250; 128; 0
-        Interactive Marker Size: 0
-        Joint Violation Color: 255; 0; 255
-        Planning Group: arm
-        Query Goal State: false
-        Query Start State: false
-        Show Workspace: false
-        Start State Alpha: 1
-        Start State Color: 0; 255; 0
-      Planning Scene Topic: /move_group/monitored_planning_scene
-      Robot Description: robot_description
-      Scene Geometry:
-        Scene Alpha: 1
-        Scene Color: 50; 230; 50
-        Scene Display Time: 0.2
-        Show Scene Geometry: true
-        Voxel Coloring: Z-Axis
-        Voxel Rendering: Occupied Voxels
-      Scene Robot:
-        Attached Body Color: 150; 50; 150
-        Links:
-          All Links Enabled: true
-          Expand Joint Details: false
-          Expand Link Details: false
-          Expand Tree: false
-          Link Tree Style: Links in Alphabetic Order
-          arm_base_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          arm_elbow_F10_0_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          arm_elbow_F10_1_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          arm_elbow_F10_2_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          arm_elbow_F3_0_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          arm_elbow_flex_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          arm_elbow_flex_servo_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          arm_shoulder_F10_0_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          arm_shoulder_F10_1_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          arm_shoulder_F10_2_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          arm_shoulder_F3_0_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          arm_shoulder_lift_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          arm_shoulder_lift_servo_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          arm_shoulder_pan_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          arm_shoulder_pan_servo_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          arm_wrist_F3_0_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          arm_wrist_flex_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          arm_wrist_flex_servo_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          base_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          gripper_active_finger_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          gripper_active_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          gripper_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          gripper_servo_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          gripper_static_finger_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          gripper_static_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-        Robot Alpha: 0.5
-        Show Robot Collision: false
-        Show Robot Visual: true
-      Value: true
-    - Alpha: 1
-      Axes Length: 1
-      Axes Radius: 0.1
-      Class: rviz/Pose
-      Color: 220; 220; 220
-      Enabled: true
-      Head Length: 0.02
-      Head Radius: 0.02
-      Name: Pick/Place Pose
-      Shaft Length: 0.08
-      Shaft Radius: 0.01
-      Shape: Arrow
-      Topic: /gripper_pose
-      Value: true
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz/PointCloud2
-      Color: 71; 255; 15
-      Color Transformer: RGB8
-      Decay Time: 0
-      Enabled: false
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Max Intensity: 4096
-      Min Color: 0; 0; 0
-      Min Intensity: 0
-      Name: PointCloud2
-      Position Transformer: XYZ
-      Queue Size: 10
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.005
-      Style: Flat Squares
-      Topic: /camera/depth_registered/points
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: false
-    - Alpha: 1
-      Auto Size:
-        Auto Size Factor: 1
-        Value: true
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz/DepthCloud
-      Color: 255; 255; 255
-      Color Image Topic: /camera/rgb/image_color
-      Color Transformer: RGB8
-      Color Transport Hint: raw
-      Decay Time: 0
-      Depth Map Topic: /camera/depth_registered/image_raw
-      Depth Map Transport Hint: raw
-      Enabled: true
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Max Intensity: 4096
-      Min Color: 0; 0; 0
-      Min Intensity: 0
-      Name: DepthCloud
-      Occlusion Compensation:
-        Occlusion Time-Out: 30
-        Value: false
-      Position Transformer: XYZ
-      Queue Size: 5
-      Selectable: true
-      Size (Pixels): 3
-      Style: Flat Squares
-      Topic Filter: true
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: true
-    - Arrow Length: 0.3
-      Class: rviz/PoseArray
-      Color: 0; 0; 255
-      Enabled: true
-      Name: Blocks Poses
-      Topic: /turtlebot_blocks
-      Value: true
-    - Arrow Length: 0.3
-      Class: rviz/PoseArray
-      Color: 255; 25; 0
-      Enabled: true
-      Name: Pick & Place Poses
-      Topic: /pick_and_place
-      Value: true
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: RGB8
-      Decay Time: 0
-      Enabled: true
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Max Intensity: 4096
-      Min Color: 0; 0; 0
-      Min Intensity: 0
-      Name: Blocks PointCloud
-      Position Transformer: XYZ
-      Queue Size: 10
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.01
-      Style: Flat Squares
-      Topic: /block_detection_action_server/block_output
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: true
-    - Class: rviz/InteractiveMarkers
-      Enable Transparency: true
-      Enabled: true
-      Name: InteractiveMarkers
-      Show Axes: true
-      Show Descriptions: true
-      Show Visual Aids: true
-      Update Topic: /block_controls/update
-      Value: true
-    - Alpha: 1
-      Axes Length: 0.1
-      Axes Radius: 0.008
-      Class: rviz/Pose
-      Color: 255; 25; 0
-      Enabled: true
-      Head Length: 0.3
-      Head Radius: 0.1
-      Name: Pose
-      Shaft Length: 1
-      Shaft Radius: 0.05
-      Shape: Axes
-      Topic: /target_pose
-      Value: true
-    - Class: rviz/TF
-      Enabled: false
-      Frame Timeout: 15
-      Frames:
-        All Enabled: false
-      Marker Scale: 1
-      Name: TF
-      Show Arrows: true
-      Show Axes: true
-      Show Names: true
-      Tree:
-        {}
-      Update Interval: 0
-      Value: false
-  Enabled: true
-  Global Options:
-    Background Color: 48; 48; 48
-    Fixed Frame: base_link
-    Frame Rate: 30
-  Name: root
-  Tools:
-    - Class: rviz/Interact
-      Hide Inactive Objects: true
-    - Class: rviz/MoveCamera
-    - Class: rviz/Select
-  Value: true
-  Views:
-    Current:
-      Class: rviz/XYOrbit
-      Distance: 1.99167
-      Enable Stereo Rendering:
-        Stereo Eye Separation: 0.06
-        Stereo Focal Distance: 1
-        Swap Stereo Eyes: false
-        Value: false
-      Focal Point:
-        X: 0.432322
-        Y: 0.135766
-        Z: 8.08975e-07
-      Name: Current View
-      Near Clip Distance: 0.01
-      Pitch: 0.429801
-      Target Frame: base_link
-      Value: XYOrbit (rviz)
-      Yaw: 5.53498
-    Saved: ~
-Window Geometry:
-  Displays:
-    collapsed: false
-  Height: 990
-  Help:
-    collapsed: false
-  Hide Left Dock: false
-  Hide Right Dock: false
-  Motion Planning:
-    collapsed: false
-  QMainWindow State: 000000ff00000000fd00000001000000000000022a0000038efc0200000006fb000000100044006900730070006c0061007900730100000033000001d1000000d801000005fb0000000800480065006c00700000000342000000bb0000007101000005fb0000000a0056006900650077007300000003b0000000b0000000ab01000005fb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000207000001ba0000015601000005fb0000000a0049006d006100670065000000030a000000b70000000000000000000005530000038e00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
-  Views:
-    collapsed: false
-  Width: 1920
-  X: 0
-  Y: 35

src/turtlebot_arm/turtlebot_arm_block_manipulation/demo/block_manipulation_moveit.launch

@@ -1,23 +0,0 @@
-<launch>
-
-  <!--  ************* Arm bringup stuff ************* -->
-
-  <include file="$(find turtlebot_arm_bringup)/launch/arm.launch" />
-
-  <node name="arbotix_gui" pkg="arbotix_python" type="arbotix_gui" output="screen"/>
-
-
-  <!--  ************* Moveit config stuff *************  -->
-
-  <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
-  <include file="$(find turtlebot_arm_moveit_config)/launch/planning_context.launch">
-    <arg name="load_robot_description" value="true"/>
-  </include>
-
-  <!-- Run the main MoveIt executable to provide move groups -->
-  <include file="$(find turtlebot_arm_moveit_config)/launch/move_group.launch">
-    <arg name="allow_trajectory_execution" value="true"/>  
-    <arg name="fake_execution" value="false"/>
-    <arg name="info" value="true"/>
-  </include>
-</launch>

src/turtlebot_arm/turtlebot_arm_block_manipulation/launch/block_manipulation.launch

@@ -1,14 +0,0 @@
-<launch>
-  <!-- Launch all the action servers for the block manipulation demo -->
-
-  <node name="block_detection_action_server" pkg="turtlebot_arm_block_manipulation" type="block_detection_action_server" output="screen">
-    <rosparam param="table_pose">[0.09, 0.0, -0.035]</rosparam>
-  </node>
-
-  <node name="pick_and_place_action_server" pkg="turtlebot_arm_block_manipulation" type="pick_and_place_action_server" output="screen">
-  </node>
-  
-  <node name="interactive_manipulation_action_server" pkg="turtlebot_arm_block_manipulation" type="interactive_manipulation_action_server" output="screen">
-  </node>
-
-</launch>

src/turtlebot_arm/turtlebot_arm_block_manipulation/package.xml

@@ -1,37 +0,0 @@
-<package>
-  <name>turtlebot_arm_block_manipulation</name>
-  <version>0.3.3</version>
-  <description>
-    turtlebot_arm_block_manipulation contains a demo allowing the TurtleBot arm
-    to manipulate small blocks on a level surface using interactive markers.
-  </description>
-
-  <author>Michael Ferguson</author>
-  <author>Helen Oleynikova</author>
-  <maintainer email="jsantossimon@gmail.com">Jorge Santos</maintainer>
-  <license>BSD</license>
-
-  <url type="website">http://ros.org/wiki/turtlebot_arm_block_manipulation</url>
-  <url type="repository">https://github.com/turtlebot/turtlebot_arm</url>
-  <url type="bugtracker">https://github.com/turtlebot/turtlebot_arm/issues</url>
-  
-  <buildtool_depend>catkin</buildtool_depend>
-
-  <build_depend>roscpp</build_depend>
-  <build_depend>actionlib</build_depend>
-  <build_depend>actionlib_msgs</build_depend>
-  <build_depend>interactive_markers</build_depend>
-  <build_depend>visualization_msgs</build_depend>
-  <build_depend>pcl_ros</build_depend>
-  <build_depend>moveit_core</build_depend>
-  <build_depend>moveit_ros_planning_interface</build_depend>
-
-  <run_depend>roscpp</run_depend>
-  <run_depend>actionlib</run_depend>
-  <run_depend>actionlib_msgs</run_depend>
-  <run_depend>interactive_markers</run_depend>
-  <run_depend>visualization_msgs</run_depend>
-  <run_depend>pcl_ros</run_depend>
-  <run_depend>moveit_core</run_depend>
-  <run_depend>moveit_ros_planning_interface</run_depend>
-</package>

src/turtlebot_arm/turtlebot_arm_block_manipulation/src/block_detection_action_server.cpp

@@ -1,397 +0,0 @@
-/*
- * Copyright (c) 2011, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- *     * Redistributions of source code must retain the above copyright
- *       notice, this list of conditions and the following disclaimer.
- *     * Redistributions in binary form must reproduce the above copyright
- *       notice, this list of conditions and the following disclaimer in the
- *       documentation and/or other materials provided with the distribution.
- *     * Neither the name of the Willow Garage, Inc. nor the names of its
- *       contributors may be used to endorse or promote products derived from
- *       this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- * 
- * Author: Michael Ferguson, Helen Oleynikova
- */
-
-#include <ros/ros.h>
-#include <sensor_msgs/PointCloud2.h>
-#include <actionlib/server/simple_action_server.h>
-#include <turtlebot_arm_block_manipulation/BlockDetectionAction.h>
-
-#include <tf/transform_listener.h>
-
-#include <pcl/conversions.h>
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-#include <pcl/filters/passthrough.h>
-#include <pcl/segmentation/extract_clusters.h>
-#include <pcl/filters/extract_indices.h>
-#include <pcl/kdtree/kdtree_flann.h>
-#include <pcl/sample_consensus/method_types.h>
-#include <pcl/sample_consensus/model_types.h>
-#include <pcl/segmentation/sac_segmentation.h>
-#include <pcl/segmentation/extract_clusters.h>
-
-#include <pcl_ros/point_cloud.h>
-#include <pcl_ros/transforms.h>
-
-// MoveIt!
-#include <moveit/planning_scene_interface/planning_scene_interface.h>
-
-#include <cmath>
-#include <algorithm>
-
-namespace turtlebot_arm_block_manipulation
-{
-
-class BlockDetectionServer
-{
-private:
-    
-  ros::NodeHandle nh_;
-  actionlib::SimpleActionServer<turtlebot_arm_block_manipulation::BlockDetectionAction> as_;
-  std::string action_name_;
-  turtlebot_arm_block_manipulation::BlockDetectionFeedback feedback_;
-  turtlebot_arm_block_manipulation::BlockDetectionResult result_;
-  turtlebot_arm_block_manipulation::BlockDetectionGoalConstPtr goal_;
-  ros::Subscriber sub_;
-  ros::Publisher pub_;
-
-  // We use the planning_scene_interface::PlanningSceneInterface to manipulate the world
-  moveit::planning_interface::PlanningSceneInterface planning_scene_interface_;
-
-  tf::TransformListener tf_listener_;
-  
-  // Parameters from goal
-  std::string arm_link_;
-  double block_size_;
-  double table_height_;
-  
-  ros::Publisher block_pub_;
-  ros::Publisher c_obj_pub_;
-  
-  // Parameters from node
-  std::vector<double> table_pose_;
-
-  // Constants
-  const double TABLE_SIZE_X = 0.5;
-  const double TABLE_SIZE_Y = 1.0;
-  const double TABLE_SIZE_Z = 0.05;
-
-  
-public:
-  BlockDetectionServer(const std::string name) : 
-    nh_("~"), as_(name, false), action_name_(name)
-  {
-    // Load parameters from the server.
-    if ((nh_.getParam("table_pose", table_pose_) == true) && (table_pose_.size() != 3))
-    {
-      ROS_ERROR("Invalid table_pose vector size; must contain 3 values (x, y, z); ignoring");
-      table_pose_.clear();
-    }
-    
-    // Register the goal and feeback callbacks.
-    as_.registerGoalCallback(boost::bind(&BlockDetectionServer::goalCB, this));
-    as_.registerPreemptCallback(boost::bind(&BlockDetectionServer::preemptCB, this));
-    
-    as_.start();
-    
-    // Subscribe to point cloud
-    sub_ = nh_.subscribe("/camera/depth_registered/points", 1, &BlockDetectionServer::cloudCb, this);
-
-    // Publish the filtered point cloud for debug purposes
-    pub_ = nh_.advertise< pcl::PointCloud<pcl::PointXYZRGB> >("block_output", 1);
-
-    // Public detected blocks poses
-    block_pub_ = nh_.advertise<geometry_msgs::PoseArray>("/turtlebot_blocks", 1, true);
-  }
-
-  void goalCB()
-  {
-    ROS_INFO("[block detection] Received goal!");
-    // accept the new goal
-    result_.blocks.poses.clear();
-    
-    goal_ = as_.acceptNewGoal();
-    
-    block_size_ = goal_->block_size;
-    table_height_ = goal_->table_height;
-    arm_link_ = goal_->frame;
-
-    result_.blocks.header.frame_id = arm_link_;
-
-    // Add the table as a collision object, so it gets filtered out from MoveIt! octomap
-    // I let it optional, as I don't know if this will work in most cases, honesty speaking
-    if (table_pose_.size() == 3)
-    {
-      if (std::abs(table_height_ - table_pose_[2]) > 0.05)
-        ROS_WARN("The table height (%f) passed with goal and table_pose[2] parameter (%f) " \
-                 "should be very similar", table_height_, table_pose_[2]);
-      addTable();
-    }
-  }
-
-  void preemptCB()
-  {
-    ROS_INFO("%s: Preempted", action_name_.c_str());
-    // set the action state to preempted
-    as_.setPreempted();
-  }
-
-  void cloudCb(const sensor_msgs::PointCloud2ConstPtr& msg)
-  {
-    // Only do this if we're actually actively working on a goal.
-    if (!as_.isActive())
-      return;
-
-    result_.blocks.header.stamp = msg->header.stamp;
-
-    // convert to PCL
-    pcl::PointCloud < pcl::PointXYZRGB > cloud;
-    pcl::fromROSMsg(*msg, cloud);
-
-    // transform to whatever frame we're working in, probably the arm frame.
-    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_transformed(new pcl::PointCloud<pcl::PointXYZRGB>);
-
-    tf_listener_.waitForTransform(std::string(arm_link_), cloud.header.frame_id,
-                                  ros::Time(cloud.header.stamp), ros::Duration(1.0));
-    if (!pcl_ros::transformPointCloud(std::string(arm_link_), cloud, *cloud_transformed, tf_listener_))
-    {
-      ROS_ERROR("Error converting to desired frame");
-      return;
-    }
-
-    // Create the segmentation object for the planar model and set all the parameters
-    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
-    pcl::SACSegmentation<pcl::PointXYZRGB> seg;
-    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
-    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
-    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZRGB>());
-    seg.setOptimizeCoefficients(true);
-    seg.setModelType(pcl::SACMODEL_PLANE);
-    seg.setMethodType(pcl::SAC_RANSAC);
-    seg.setMaxIterations(200);
-    seg.setDistanceThreshold(0.005);
-
-    // Limit to things we think are roughly at the table height.
-    pcl::PassThrough<pcl::PointXYZRGB> pass;
-    pass.setInputCloud(cloud_transformed);
-    pass.setFilterFieldName("z");
-
-    pass.setFilterLimits(table_height_ - 0.05, table_height_ + block_size_ + 0.05);
-    pass.filter(*cloud_filtered);
-    if (cloud_filtered->points.size() == 0)
-    {
-      ROS_ERROR("0 points left");
-      return;
-    }
-    else
-      ROS_INFO("Filtered, %d points left", (int ) cloud_filtered->points.size());
-
-    int nr_points = cloud_filtered->points.size ();
-    while (cloud_filtered->points.size() > 0.3 * nr_points)
-    {
-      // Segment the largest planar component from the remaining cloud
-      seg.setInputCloud(cloud_filtered);
-      seg.segment(*inliers, *coefficients);
-      if (inliers->indices.size() == 0)
-      {
-        std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
-        return;
-      }
-
-      std::cout << "Inliers: " << (inliers->indices.size()) << std::endl;
-
-      // Extract the planar inliers from the input cloud
-      pcl::ExtractIndices<pcl::PointXYZRGB> extract;
-      extract.setInputCloud(cloud_filtered);
-      extract.setIndices(inliers);
-      extract.setNegative(false);
-
-      // Write the planar inliers to disk
-      extract.filter(*cloud_plane);
-      std::cout << "PointCloud representing the planar component: "
-                << cloud_plane->points.size() << " data points." << std::endl;
-
-      // Remove the planar inliers, extract the rest
-      extract.setNegative(true);
-      extract.filter(*cloud_filtered);
-    }
-
-    // Creating the KdTree object for the search method of the extraction
-    pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>);
-    tree->setInputCloud(cloud_filtered);
-
-    std::vector<pcl::PointIndices> cluster_indices;
-    pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
-    ec.setClusterTolerance(0.005);
-    ec.setMinClusterSize(100);
-    ec.setMaxClusterSize(25000);
-    ec.setSearchMethod(tree);
-    ec.setInputCloud(cloud_filtered);
-    ec.extract(cluster_indices);
-
-    pub_.publish(cloud_filtered);
-
-    // for each cluster, see if it is a block
-    for (size_t c = 0; c < cluster_indices.size (); ++c)
-    {  
-      // find the outer dimensions of the cluster
-      float xmin = 0; float xmax = 0;
-      float ymin = 0; float ymax = 0;
-      float zmin = 0; float zmax = 0;
-
-      for (size_t i = 0; i < cluster_indices[c].indices.size(); i++)
-      {
-          int j = cluster_indices[c].indices[i];
-          float x = cloud_filtered->points[j].x;
-          float y = cloud_filtered->points[j].y;
-          float z = cloud_filtered->points[j].z;
-          if (i == 0)
-          {
-            xmin = xmax = x;
-            ymin = ymax = y;
-            zmin = zmax = z;
-          }
-          else
-          {
-            xmin = std::min(xmin, x);
-            xmax = std::max(xmax, x);
-            ymin = std::min(ymin, y);
-            ymax = std::max(ymax, y);
-            zmin = std::min(zmin, z);
-            zmax = std::max(zmax, z);
-          }    
-      }
-      
-      // Check if these dimensions make sense for the block size specified
-      float xside = xmax-xmin;
-      float yside = ymax-ymin;
-      float zside = zmax-zmin;
-      
-      const float tol = 0.01; // 1 cm error tolerance
-      // In order to be part of the block, xside and yside must be between
-      // blocksize and blocksize*sqrt(2)
-      // z must be equal to or smaller than blocksize
-      if (xside > block_size_ - tol && xside < block_size_*sqrt(2) + tol &&
-          yside > block_size_ - tol && yside < block_size_*sqrt(2) + tol &&
-          zside > tol && zside < block_size_ + tol)
-      {
-        float x = xmin + xside/2.0;
-        float y = ymin + yside/2.0;
-        float z = zmax - block_size_/2.0;
-
-        // Discard blocks outside the table limits (normally something detected on the robot)
-        // Note that table pose x marks the table border closest to arm_link_ frame, not its center
-        if ((table_pose_.size() == 3) &&
-            ((x < table_pose_[0]) || (x > table_pose_[0] + TABLE_SIZE_X) ||
-             (y < table_pose_[1] - TABLE_SIZE_Y/2.0) || (y > table_pose_[1] + TABLE_SIZE_Y/2.0)))
-        {
-          ROS_DEBUG_STREAM("Block at [" << x << ", " << y << "] outside table limits ["
-                         << table_pose_[0] - TABLE_SIZE_X/2.0 << ", " << table_pose_[0] + TABLE_SIZE_X/2.0 << ", "
-                         << table_pose_[1] - TABLE_SIZE_Y/2.0 << ", " << table_pose_[1] + TABLE_SIZE_Y/2.0);
-          continue;
-        }
-
-        // If so, then figure out the position and the orientation of the block
-        float angle = atan(block_size_/((xside + yside)/2));
-        
-        if (yside < block_size_)
-          angle = 0.0;
-        
-        ROS_INFO_STREAM("xside: " << xside << " yside: " << yside << " zside " << zside << " angle: " << angle);
-        // Then add it to our set
-        ROS_INFO_STREAM("Adding a new block at [" << x << ", " << y << ", " << z << ", " << angle << "]");
-        addBlock(x, y, z, angle);
-      }
-    }
-    
-    if (result_.blocks.poses.size() > 0)
-    {
-      as_.setSucceeded(result_);
-      block_pub_.publish(result_.blocks);
-      ROS_INFO("[block detection] Set as succeeded!");
-    }
-    else
-      ROS_INFO("[block detection] Couldn't find any blocks this iteration!");
-    //  as_.setAborted(result_);
-  }
-
-
-private:
-
-  void addBlock(float x, float y, float z, float angle)
-  {
-    geometry_msgs::Pose block_pose;
-    block_pose.position.x = x;
-    block_pose.position.y = y;
-    block_pose.position.z = z;
-    
-    Eigen::Quaternionf quat(Eigen::AngleAxis<float>(angle, Eigen::Vector3f(0,0,1)));
-    
-    block_pose.orientation.x = quat.x();
-    block_pose.orientation.y = quat.y();
-    block_pose.orientation.z = quat.z();
-    block_pose.orientation.w = quat.w();
-    
-    result_.blocks.poses.push_back(block_pose);
-  }
-
-  void addTable()
-  {
-    // Add the table as a collision object into the world, so it gets excluded from the collision map
-    // As the blocks are small, they should also be excluded (assuming that padding_offset parameter on
-    // octomap sensor configuration is equal or bigger than block size)
-    moveit_msgs::CollisionObject co;
-    co.header.stamp = ros::Time::now();
-    co.header.frame_id = arm_link_;
-
-    co.id = "table";
-    planning_scene_interface_.removeCollisionObjects(std::vector<std::string>(1, co.id));
-
-    co.operation = moveit_msgs::CollisionObject::ADD;
-    co.primitives.resize(1);
-    co.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
-    co.primitives[0].dimensions.resize(3);
-    co.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = TABLE_SIZE_X;
-    co.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = TABLE_SIZE_Y;
-    co.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = TABLE_SIZE_Z;
-    co.primitive_poses.resize(1);
-    co.primitive_poses[0].position.x = table_pose_[0] + TABLE_SIZE_X/2.0;
-    co.primitive_poses[0].position.y = table_pose_[1];
-    co.primitive_poses[0].position.z = table_pose_[2] - TABLE_SIZE_Z/2.0;
-
-    ROS_INFO("Add the table as a collision object into the world");
-    std::vector<moveit_msgs::CollisionObject> collision_objects(1, co);
-    planning_scene_interface_.addCollisionObjects(collision_objects);
-  }
-
-};
-
-};
-
-int main(int argc, char** argv)
-{
-  ros::init(argc, argv, "block_detection_action_server");
-
-  turtlebot_arm_block_manipulation::BlockDetectionServer server("block_detection");
-  ros::spin();
-
-  return 0;
-}

src/turtlebot_arm/turtlebot_arm_block_manipulation/src/interactive_manipulation_action_server.cpp

@@ -1,253 +0,0 @@
-/*
- * Copyright (c) 2011, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- *     * Redistributions of source code must retain the above copyright
- *       notice, this list of conditions and the following disclaimer.
- *     * Redistributions in binary form must reproduce the above copyright
- *       notice, this list of conditions and the following disclaimer in the
- *       documentation and/or other materials provided with the distribution.
- *     * Neither the name of the Willow Garage, Inc. nor the names of its
- *       contributors may be used to endorse or promote products derived from
- *       this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- * 
- * Author: Michael Ferguson, Helen Oleynikova
- */
-
-#include <ros/ros.h>
-
-#include <actionlib/server/simple_action_server.h>
-#include <interactive_markers/interactive_marker_server.h>
-
-#include <turtlebot_arm_block_manipulation/InteractiveBlockManipulationAction.h>
-#include <geometry_msgs/PoseArray.h>
-
-using namespace visualization_msgs;
-
-namespace turtlebot_arm_block_manipulation
-{
-
-class InteractiveManipulationServer
-{
-private:
-  ros::NodeHandle nh_;
-
-  interactive_markers::InteractiveMarkerServer server_;
-  
-  actionlib::SimpleActionServer<turtlebot_arm_block_manipulation::InteractiveBlockManipulationAction> as_;
-  std::string action_name_;
-  
-  turtlebot_arm_block_manipulation::InteractiveBlockManipulationFeedback     feedback_;
-  turtlebot_arm_block_manipulation::InteractiveBlockManipulationResult       result_;
-  turtlebot_arm_block_manipulation::InteractiveBlockManipulationGoalConstPtr goal_;
-  
-  ros::Subscriber block_sub_;
-  ros::Publisher  pick_and_place_pub_;
-
-  geometry_msgs::Pose old_pose_;
-  
-  geometry_msgs::PoseArrayConstPtr msg_;
-  bool initialized_;
-  
-  // Parameters from goal
-  std::string arm_link;
-  double      block_size;
-  
-  // Parameters from server
-  double bump_size;
-
-public:
-
-  InteractiveManipulationServer(const std::string name) : 
-     nh_("~"), server_("block_controls"), as_(name, false), action_name_(name), initialized_(false), block_size(0)
-  {
-    // Load parameters from the server.
-    nh_.param<double>("bump_size", bump_size, 0.005);
-    
-    // Register the goal and feeback callbacks.
-    as_.registerGoalCallback(boost::bind(&InteractiveManipulationServer::goalCB, this));
-    as_.registerPreemptCallback(boost::bind(&InteractiveManipulationServer::preemptCB, this));
-    
-    as_.start();
-  
-    block_sub_ = nh_.subscribe("/turtlebot_blocks", 1, &InteractiveManipulationServer::addBlocks, this);
-    pick_and_place_pub_ = nh_.advertise< geometry_msgs::PoseArray >("/pick_and_place", 1, true);
-  }
-  
-  void goalCB()
-  {
-
-    // accept the new goal
-    goal_ = as_.acceptNewGoal();
-    
-    ROS_INFO("[interactive manipulation] Received goal! %f, %s", goal_->block_size, goal_->frame.c_str());
-    
-    block_size = goal_->block_size;
-    arm_link = goal_->frame;
-    
-    if (initialized_)
-      addBlocks(msg_);
-  }
-
-  void preemptCB()
-  {
-    ROS_INFO("%s: Preempted", action_name_.c_str());
-    // set the action state to preempted
-    as_.setPreempted();
-  }
-  
-  void addBlocks(const geometry_msgs::PoseArrayConstPtr& msg)
-  {
-    server_.clear();
-    server_.applyChanges();
-  
-    ROS_INFO("Got block detection callback. Adding blocks.");
-    geometry_msgs::Pose block;
-    bool active = as_.isActive();
-    
-    for (unsigned int i=0; i < msg->poses.size(); i++)
-    {
-      block = msg->poses[i];
-      addBlock(block, i, active, msg->header.frame_id);
-      ROS_INFO("Added %d blocks", i);
-    }
-    
-    server_.applyChanges();
-    
-    msg_ = msg;
-    initialized_ = true;
-  }
-  
-
-  // Move the real block!
-  void feedbackCb( const InteractiveMarkerFeedbackConstPtr &feedback )
-  {
-    if (!as_.isActive())
-    {
-      ROS_INFO("Got feedback but not active!");
-      return;
-    }
-    switch ( feedback->event_type )
-    {
-      case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN:
-        ROS_INFO_STREAM("Staging " << feedback->marker_name);     
-        old_pose_ = feedback->pose;
-        break;
-   
-      case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP:
-        ROS_INFO_STREAM("Now moving " << feedback->marker_name); 
-        moveBlock(old_pose_, feedback->pose);
-        break;
-    }
-    
-    server_.applyChanges(); 
-  }
-  
-  void moveBlock(const geometry_msgs::Pose& start_pose, const geometry_msgs::Pose& end_pose)
-  {
-    // Return pickup and place poses as the result of the action
-    geometry_msgs::Pose start_pose_bumped, end_pose_bumped;
-    start_pose_bumped = start_pose;
-    //  start_pose_bumped.position.y -= bump_size;  TODO/WARN: this was in the original version, but destroys piss off indigo/moveit operation!
-    start_pose_bumped.position.z -= block_size/2.0 - bump_size;
-
-    result_.pickup_pose = start_pose_bumped;
-    
-    end_pose_bumped = end_pose;
-    end_pose_bumped.position.z -= block_size/2.0 - bump_size;
-    result_.place_pose = end_pose_bumped;
-    
-    // Publish pickup and place poses for visualizing on RViz
-    geometry_msgs::PoseArray msg;
-    msg.header.frame_id = arm_link;
-    msg.header.stamp = ros::Time::now();
-    msg.poses.push_back(start_pose_bumped);
-    msg.poses.push_back(end_pose_bumped);
-    
-    pick_and_place_pub_.publish(msg);
-    
-    as_.setSucceeded(result_);
-    
-    server_.clear();
-    server_.applyChanges();
-  }
-
-  // Make a box
-  Marker makeBox( InteractiveMarker &msg, float r, float g, float b )
-  {
-    Marker m;
-
-    m.type = Marker::CUBE;
-    m.scale.x = msg.scale;
-    m.scale.y = msg.scale;
-    m.scale.z = msg.scale;
-    m.color.r = r;
-    m.color.g = g;
-    m.color.b = b;
-    m.color.a = 1.0;
-
-    return m;
-  }
-   
-  // Add a new block
-  void addBlock( const geometry_msgs::Pose pose, int n, bool active, std::string link)
-  {
-    InteractiveMarker marker;
-    marker.header.frame_id = link;
-    marker.pose = pose;
-    marker.scale = block_size;
-    
-    std::stringstream conv;
-    conv << n;
-    conv.str();
-     
-    marker.name = "block" + conv.str();
-
-    InteractiveMarkerControl control;
-    control.orientation.w = 1;
-    control.orientation.x = 0;
-    control.orientation.y = 0;
-    control.orientation.z = 0;
-    control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
-    
-    if (active)
-      marker.controls.push_back( control );
-    
-    control.markers.push_back( makeBox(marker, .5, .5, .5) );
-    control.always_visible = true;
-    marker.controls.push_back( control );
-    
-
-    server_.insert( marker );
-    server_.setCallback( marker.name, boost::bind( &InteractiveManipulationServer::feedbackCb, this, _1 ));
-  }
-
-};
-
-};
-
-int main(int argc, char** argv)
-{
-  // initialize node
-  ros::init(argc, argv, "interactive_manipulation_action_server");
-
-  turtlebot_arm_block_manipulation::InteractiveManipulationServer manip("interactive_manipulation");
-
-  ros::spin();
-}
-

src/turtlebot_arm/turtlebot_arm_block_manipulation/src/pick_and_place_action_server.cpp

@@ -1,329 +0,0 @@
-/*
- * Copyright (c) 2011, Vanadium Labs LLC
- * All Rights Reserved
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- *     * Redistributions of source code must retain the above copyright
- *       notice, this list of conditions and the following disclaimer.
- *     * Redistributions in binary form must reproduce the above copyright
- *       notice, this list of conditions and the following disclaimer in the
- *       documentation and/or other materials provided with the distribution.
- *     * Neither the name of Vanadium Labs LLC nor the names of its
- *       contributors may be used to endorse or promote products derived
- *       from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
- * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
- * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
- * DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
- * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
- * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
- * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
- * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
- * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- * Author: Michael Ferguson, Helen Oleynikova
- */
-
-#include <ros/ros.h>
-#include <tf/tf.h>
-
-#include <actionlib/server/simple_action_server.h>
-#include <turtlebot_arm_block_manipulation/PickAndPlaceAction.h>
-
-#include <moveit/move_group_interface/move_group_interface.h>
-
-#include <geometry_msgs/PoseArray.h>
-
-namespace turtlebot_arm_block_manipulation
-{
-
-class PickAndPlaceServer
-{
-private:
-
-  ros::NodeHandle nh_;
-  actionlib::SimpleActionServer<turtlebot_arm_block_manipulation::PickAndPlaceAction> as_;
-  std::string action_name_;
-
-  turtlebot_arm_block_manipulation::PickAndPlaceFeedback     feedback_;
-  turtlebot_arm_block_manipulation::PickAndPlaceResult       result_;
-  turtlebot_arm_block_manipulation::PickAndPlaceGoalConstPtr goal_;
-
-  ros::Publisher target_pose_pub_;
-  ros::Subscriber pick_and_place_sub_;
-
-  // Move groups to control arm and gripper with MoveIt!
-  moveit::planning_interface::MoveGroupInterface arm_;
-  moveit::planning_interface::MoveGroupInterface gripper_;
-
-  // Pick and place parameters
-  std::string arm_link;
-  double gripper_open;
-  double gripper_closed;
-  double attach_time;
-  double detach_time;
-  double z_up;
-
-public:
-  PickAndPlaceServer(const std::string name) :
-    nh_("~"), as_(name, false), action_name_(name), arm_("arm"), gripper_("gripper")
-  {
-    // Read specific pick and place parameters
-    nh_.param("grasp_attach_time", attach_time, 0.8);
-    nh_.param("grasp_detach_time", detach_time, 0.6);
-
-    // Register the goal and feedback callbacks
-    as_.registerGoalCallback(boost::bind(&PickAndPlaceServer::goalCB, this));
-    as_.registerPreemptCallback(boost::bind(&PickAndPlaceServer::preemptCB, this));
-
-    as_.start();
-
-    target_pose_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("/target_pose", 1, true);
-  }
-
-  void goalCB()
-  {
-    ROS_INFO("[pick and place] Received goal!");
-    goal_ = as_.acceptNewGoal();
-    arm_link = goal_->frame;
-    gripper_open = goal_->gripper_open;
-    gripper_closed = goal_->gripper_closed;
-    z_up = goal_->z_up;
-
-    arm_.setPoseReferenceFrame(arm_link);
-
-    // Allow some leeway in position (meters) and orientation (radians)
-    arm_.setGoalPositionTolerance(0.001);
-    arm_.setGoalOrientationTolerance(0.1);
-
-    // Allow replanning to increase the odds of a solution
-    arm_.allowReplanning(true);
-
-    if (goal_->topic.length() < 1)
-      pickAndPlace(goal_->pickup_pose, goal_->place_pose);
-    else
-      pick_and_place_sub_ = nh_.subscribe(goal_->topic, 1, &PickAndPlaceServer::sendGoalFromTopic, this);
-  }
-
-  void sendGoalFromTopic(const geometry_msgs::PoseArrayConstPtr& msg)
-  {
-    ROS_INFO("[pick and place] Got goal from topic! %s", goal_->topic.c_str());
-    pickAndPlace(msg->poses[0], msg->poses[1]);
-    pick_and_place_sub_.shutdown();
-  }
-
-  void preemptCB()
-  {
-    ROS_INFO("%s: Preempted", action_name_.c_str());
-    // set the action state to preempted
-    as_.setPreempted();
-  }
-
-  void pickAndPlace(const geometry_msgs::Pose& start_pose, const geometry_msgs::Pose& end_pose)
-  {
-    ROS_INFO("[pick and place] Picking. Also placing.");
-
-    geometry_msgs::Pose target;
-
-    /* open gripper */
-    if (setGripper(gripper_open) == false)
-      return;
-
-    /* hover over */
-    target = start_pose;
-    target.position.z = z_up;
-    if (moveArmTo(target) == false)
-      return;
-
-    /* go down */
-    target.position.z = start_pose.position.z;
-    if (moveArmTo(target) == false)
-      return;
-
-    /* close gripper */
-    if (setGripper(gripper_closed) == false)
-      return;
-    ros::Duration(attach_time).sleep(); // ensure that gripper properly grasp the cube before lifting the arm
-
-    /* go up */
-    target.position.z = z_up;
-    if (moveArmTo(target) == false)
-      return;
-
-    /* hover over */
-    target = end_pose;
-    target.position.z = z_up;
-    if (moveArmTo(target) == false)
-      return;
-
-    /* go down */
-    target.position.z = end_pose.position.z;
-    if (moveArmTo(target) == false)
-      return;
-
-    /* open gripper */
-    if (setGripper(gripper_open) == false)
-      return;
-    ros::Duration(detach_time).sleep(); // ensure that gripper properly release the cube before lifting the arm
-
-    /* go up */
-    target.position.z = z_up;
-    if (moveArmTo(target) == false)
-      return;
-
-    as_.setSucceeded(result_);
-  }
-
-private:
-  /**
-   * Move arm to a named configuration, normally described in the robot semantic description SRDF file.
-   * @param target Named target to achieve
-   * @return True of success, false otherwise
-   */
-  bool moveArmTo(const std::string& target)
-  {
-    ROS_DEBUG("[pick and place] Move arm to '%s' position", target.c_str());
-    if (arm_.setNamedTarget(target) == false)
-    {
-      ROS_ERROR("[pick and place] Set named target '%s' failed", target.c_str());
-      return false;
-    }
-
-    moveit::planning_interface::MoveItErrorCode result = arm_.move();
-    if (bool(result) == true)
-    {
-      return true;
-    }
-    else
-    {
-      ROS_ERROR("[pick and place] Move to target failed (error %d)", result.val);
-      as_.setAborted(result_);
-      return false;
-    }
-  }
-
-  /**
-   * Move arm to a target pose. Only position coordinates are taken into account; the
-   * orientation is calculated according to the direction and distance to the target.
-   * @param target Pose target to achieve
-   * @return True of success, false otherwise
-   */
-  bool moveArmTo(const geometry_msgs::Pose& target)
-  {
-    int attempts = 0;
-    ROS_DEBUG("[pick and place] Move arm to [%.2f, %.2f, %.2f, %.2f]",
-             target.position.x, target.position.y, target.position.z, tf::getYaw(target.orientation));
-    while (attempts < 5)
-    {
-      geometry_msgs::PoseStamped modiff_target;
-      modiff_target.header.frame_id = arm_link;
-      modiff_target.pose = target;
-
-      double x = modiff_target.pose.position.x;
-      double y = modiff_target.pose.position.y;
-      double z = modiff_target.pose.position.z;
-      double d = sqrt(x*x + y*y);
-      if (d > 0.3)
-      {
-        // Maximum reachable distance by the turtlebot arm is 30 cm
-        ROS_ERROR("Target pose out of reach [%f > %f]", d, 0.3);
-        as_.setAborted(result_);
-        return false;
-      }
-      // Pitch is 90 (vertical) at 10 cm from the arm base; the farther the target is, the closer to horizontal
-      // we point the gripper (0.205 = arm's max reach - vertical pitch distance + ε). Yaw is the direction to
-      // the target. We also try some random variations of both to increase the chances of successful planning.
-      // Roll is simply ignored, as our arm lacks the proper dof.
-      double rp = M_PI_2 - std::asin((d - 0.1)/0.205) + attempts*fRand(-0.05, +0.05);
-      double ry = std::atan2(y, x) + attempts*fRand(-0.05, +0.05);
-      double rr = 0.0;
-
-      tf::Quaternion q = tf::createQuaternionFromRPY(rr, rp, ry);
-      tf::quaternionTFToMsg(q, modiff_target.pose.orientation);
-
-      // Slightly increase z proportionally to pitch to avoid hitting the table with the lower gripper corner
-      ROS_DEBUG("z increase:  %f  +  %f", modiff_target.pose.position.z, std::abs(std::cos(rp))/50.0);
-      modiff_target.pose.position.z += std::abs(std::cos(rp))/50.0;
-
-      ROS_DEBUG("Set pose target [%.2f, %.2f, %.2f] [d: %.2f, r: %.2f, p: %.2f, y: %.2f]", x, y, z, d, rr, rp, ry);
-      target_pose_pub_.publish(modiff_target);
-
-      if (arm_.setPoseTarget(modiff_target) == false)
-      {
-        ROS_ERROR("Set pose target [%.2f, %.2f, %.2f, %.2f] failed",
-                  modiff_target.pose.position.x, modiff_target.pose.position.y, modiff_target.pose.position.z,
-                  tf::getYaw(modiff_target.pose.orientation));
-        as_.setAborted(result_);
-        return false;
-      }
-
-      moveit::planning_interface::MoveItErrorCode result = arm_.move();
-      if (bool(result) == true)
-      {
-        return true;
-      }
-      else
-      {
-        ROS_ERROR("[pick and place] Move to target failed (error %d) at attempt %d",
-                  result.val, attempts + 1);
-      }
-      attempts++;
-    }
-
-    ROS_ERROR("[pick and place] Move to target failed after %d attempts", attempts);
-    as_.setAborted(result_);
-    return false;
-  }
-
-  /**
-   * Set gripper opening.
-   * @param opening Physical opening of the gripper, in meters
-   * @return True of success, false otherwise
-   */
-  bool setGripper(float opening)
-  {
-    ROS_DEBUG("[pick and place] Set gripper opening to %f", opening);
-    if (gripper_.setJointValueTarget("gripper_joint", opening) == false)
-    {
-      ROS_ERROR("[pick and place] Set gripper opening to %f failed", opening);
-      return false;
-    }
-
-    moveit::planning_interface::MoveItErrorCode result = gripper_.move();
-    if (bool(result) == true)
-    {
-      return true;
-    }
-    else
-    {
-      ROS_ERROR("[pick and place] Set gripper opening failed (error %d)", result.val);
-      as_.setAborted(result_);
-      return false;
-    }
-  }
-
-  float fRand(float min, float max)
-  {
-    return ((float(rand()) / float(RAND_MAX)) * (max - min)) + min;
-  }
-};
-
-};
-
-
-int main(int argc, char** argv)
-{
-  ros::init(argc, argv, "pick_and_place_action_server");
-
-  turtlebot_arm_block_manipulation::PickAndPlaceServer server("pick_and_place");
-
-  // Setup an multi-threaded spinner as the move groups operations need continuous spinning
-  ros::MultiThreadedSpinner spinner(2);
-  spinner.spin();
-
-  return 0;
-}

src/turtlebot_arm/turtlebot_arm_block_manipulation/test/block_manipulation.cpp

@@ -1,396 +0,0 @@
-/* 
- * Copyright (c) 2011, Vanadium Labs LLC
- * All Rights Reserved
- * 
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- *     * Redistributions of source code must retain the above copyright
- *       notice, this list of conditions and the following disclaimer.
- *     * Redistributions in binary form must reproduce the above copyright
- *       notice, this list of conditions and the following disclaimer in the
- *       documentation and/or other materials provided with the distribution.
- *     * Neither the name of Vanadium Labs LLC nor the names of its 
- *       contributors may be used to endorse or promote products derived 
- *       from this software without specific prior written permission.
- * 
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
- * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
- * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
- * DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
- * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
- * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
- * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
- * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
- * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- * 
- * Author: Michael Ferguson, Helen Oleynikova
- */
-
-
-#include <ros/ros.h>
-#include <sensor_msgs/PointCloud2.h>
-
-#include <actionlib/client/simple_action_client.h>
-#include <simple_arm_server/MoveArmAction.h>
-
-#include <pcl/conversions.h>
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-#include <pcl/filters/passthrough.h>
-#include <pcl/segmentation/extract_clusters.h>
-#include <pcl/kdtree/kdtree.h>
-
-#include <pcl_ros/point_cloud.h>
-#include <pcl_ros/transforms.h>
-
-#include <interactive_markers/interactive_marker_server.h>
-
-#include <vector>
-
-
-using namespace visualization_msgs;
-
-const std::string arm_link = "/arm_base_link";
-const double gripper_open = 0.04;
-const double gripper_closed = 0.024;
-
-const double z_up = 0.08;
-const double z_down = -0.04;
-
-const double block_size = 0.0127;
-
-// Block storage
-class Block
-{
-  public:
-    int id;
-    bool active;
-    double x;
-    double y;
-    
-    Block(const Block& b) : id(b.id), active(b.active), x(b.x), y(b.y) {}
-    Block(int _id, double _x, double _y) : id(_id) , active(true), x(_x), y(_y) {}
-
-    std::string getName()
-    {
-      std::stringstream conv;
-      conv << id;
-      return std::string("block") + conv.str(); 
-    }
-};
-
-
-class BlockManipulation
-{
-private:
-  interactive_markers::InteractiveMarkerServer server;
-  actionlib::SimpleActionClient<simple_arm_server::MoveArmAction> client_;
-  ros::Publisher pub_;
-  ros::Subscriber sub_;
-  tf::TransformListener tf_listener_;
-  int markers_;
-  int moving_;
-  int skip_;
-  float x_, y_;
-  std::string last_block_;
-  std::vector<Block> marker_names_;
-  
-  ros::NodeHandle nh;
-
-public:
-
-  BlockManipulation() : server("block_controls"), client_("move_arm", true)
-  {
-    // create marker server
-    markers_ = 0;
-    last_block_ = std::string("");
-
-    ros::Duration(0.1).sleep();
-    
-    skip_ = 0;
-
-    // open gripper
-    simple_arm_server::MoveArmGoal goal;
-    simple_arm_server::ArmAction grip;
-    grip.type = simple_arm_server::ArmAction::MOVE_GRIPPER;
-    grip.command = gripper_open;
-    goal.motions.push_back(grip);
-    goal.header.frame_id = arm_link;
-    client_.sendGoal(goal);
-    client_.waitForResult(/*ros::Duration(30.0)*/);
-
-    // subscribe to point cloud
-    sub_ = nh.subscribe("/camera/depth_registered/points", 1, &BlockManipulation::cloudCb, this);
-    pub_ = nh.advertise< pcl::PointCloud<pcl::PointXYZRGB> >("block_output", 1);
-
-    server.applyChanges();
-    
-    ROS_INFO("Finished initializing.");
-  }
-
-  // Move the real block!
-  void moveBlock( const InteractiveMarkerFeedbackConstPtr &feedback )
-  {
-    switch ( feedback->event_type )
-    {
-      case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN:
-        ROS_INFO_STREAM("Staging " << feedback->marker_name);     
-        x_ = feedback->pose.position.x;
-        y_ = feedback->pose.position.y;
-        last_block_ = feedback->marker_name;
-        break;
-   
-      case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP:
-        moving_ = true;
-        ROS_INFO_STREAM("Now moving " << feedback->marker_name); 
-
-        simple_arm_server::MoveArmGoal goal;
-        simple_arm_server::ArmAction action;
-        
-        /* arm straight up */
-        tf::Quaternion temp;
-        temp.setRPY(0,1.57,0);
-        action.goal.orientation.x = temp.getX();
-        action.goal.orientation.y = temp.getY();
-        action.goal.orientation.z = temp.getZ();
-        action.goal.orientation.w = temp.getW();
-
-        /* hover over */
-        action.goal.position.x = x_;
-        action.goal.position.y = y_;
-        action.goal.position.z = z_up;
-        goal.motions.push_back(action);
-        action.move_time.sec = 1.5;
-
-        /* go down */
-        action.goal.position.z = z_down;
-        goal.motions.push_back(action);
-        action.move_time.sec = 1.5;
-
-        /* close gripper */
-        simple_arm_server::ArmAction grip;
-        grip.type = simple_arm_server::ArmAction::MOVE_GRIPPER;
-        grip.command = gripper_closed;
-        grip.move_time.sec = 1.0;
-        goal.motions.push_back(grip);
-
-        /* go up */
-        action.goal.position.z = z_up;
-        goal.motions.push_back(action);
-        action.move_time.sec = 0.25;
-
-        /* hover over */
-        action.goal.position.x = feedback->pose.position.x;
-        action.goal.position.y = feedback->pose.position.y;
-        action.goal.position.z = z_up;
-        goal.motions.push_back(action);
-        action.move_time.sec = 1.5;
-
-        /* go down */
-        action.goal.position.z = z_down;
-        goal.motions.push_back(action);
-        action.move_time.sec = 1.5;
-
-        /* open gripper */
-        grip.command = gripper_open;
-        goal.motions.push_back(grip);
-
-        /* go up */
-        action.goal.position.z = z_up;
-        goal.motions.push_back(action);
-        action.move_time.sec = 0.25;
-      
-        goal.header.frame_id = arm_link;
-        client_.sendGoal(goal);
-        client_.waitForResult(/*ros::Duration(30.0)*/);
-        /* update location */ 
-        for( std::vector<Block>::iterator it=marker_names_.begin(); it < marker_names_.end(); it++)
-        {
-          if( it->getName() == feedback->marker_name )
-          {
-            it->x = feedback->pose.position.x;
-            it->y = feedback->pose.position.y;
-            break;
-          }
-        }
-
-        moving_ = false;
-        break;
-    }
-    
-    server.applyChanges(); 
-  }
-
-  // Make a box
-  Marker makeBox( InteractiveMarker &msg, float r, float g, float b )
-  {
-    Marker m;
-
-    m.type = Marker::CUBE;
-    m.scale.x = msg.scale;
-    m.scale.y = msg.scale;
-    m.scale.z = msg.scale;
-    m.color.r = r;
-    m.color.g = g;
-    m.color.b = b;
-    m.color.a = 1.0;
-
-    return m;
-  }
-   
-  // Add a new block
-  void addBlock( float x, float y, float z, float rz, float r, float g, float b, int n)
-  {
-    InteractiveMarker marker;
-    marker.header.frame_id = arm_link;
-    marker.pose.position.x = x;
-    marker.pose.position.y = y;
-    marker.pose.position.z = z;
-    marker.scale = 0.03;
-    
-    Block block( n, x, y );
-    marker_names_.push_back( block );
-    marker.name = block.getName(); 
-    marker.description = "Another block";
-
-    InteractiveMarkerControl control;
-    control.orientation.w = 1;
-    control.orientation.x = 0;
-    control.orientation.y = 1;
-    control.orientation.z = 0;
-    control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
-    marker.controls.push_back( control );
-    
-    control.markers.push_back( makeBox(marker, r, g, b) );
-    control.always_visible = true;
-    marker.controls.push_back( control );
-    
-
-    server.insert( marker );
-    server.setCallback( marker.name, boost::bind( &BlockManipulation::moveBlock, this, _1 ));
-  }
-
-  // Process an incoming cloud
-  void cloudCb ( const sensor_msgs::PointCloud2ConstPtr& msg )
-  {
-    if( (skip_++)%15 != 0 ) return;
-    
-    // convert to PCL
-    pcl::PointCloud<pcl::PointXYZRGB> cloud;
-    pcl::fromROSMsg (*msg, cloud);
-    
-    // transform to base_link
-    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_transformed (new pcl::PointCloud<pcl::PointXYZRGB>);
-    
-    tf_listener_.waitForTransform(std::string(arm_link), cloud.header.frame_id, cloud.header.stamp, ros::Duration(1.0));
-    if (!pcl_ros::transformPointCloud (std::string(arm_link), cloud, *cloud_transformed, tf_listener_))
-    {
-      ROS_ERROR ("Error converting to base_link");
-      return;
-    }
-
-    // drop things on ground
-    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>);
-    pcl::PassThrough<pcl::PointXYZRGB> pass;
-    pass.setInputCloud(cloud_transformed); 
-    pass.setFilterFieldName("z");
-    pass.setFilterLimits(z_down - 0.01, z_down + block_size*2);
-    pass.filter(*cloud_filtered);
-    if( cloud_filtered->points.size() == 0 ){
-      ROS_ERROR("0 points left");
-      return;
-    }else
-      ROS_INFO("Filtered, %d points left", (int) cloud_filtered->points.size());
-    pub_.publish(*cloud_filtered);
-
-    // cluster
-    pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
-    tree->setInputCloud(cloud_filtered);
-    
-    std::vector<pcl::PointIndices> clusters;
-    pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
-    ec.setClusterTolerance(0.02);
-    ec.setMinClusterSize(20);
-    ec.setMaxClusterSize(25000);
-    ec.setSearchMethod(tree);
-    ec.setInputCloud(cloud_filtered);
-    ec.extract(clusters);
-
-    // need to delete old blocks
-    for( std::vector<Block>::iterator it=marker_names_.begin(); it < marker_names_.end(); it++)
-    {
-      it->active = false;
-    }
-
-    // for each cluster, see if it is a block
-    for (size_t c = 0; c < clusters.size (); ++c)
-    {  
-      // find cluster centroid/color
-      float x = 0; float y = 0; float z = 0; int r = 0; int g = 0; int b = 0;
-      for (size_t i = 0; i < clusters[c].indices.size(); i++)
-      {
-          int j = clusters[c].indices[i];
-          x += cloud_filtered->points[j].x;
-          y += cloud_filtered->points[j].y;
-          z += cloud_filtered->points[j].z;
-          
-          r += cloud_filtered->points[j].r;
-          g += cloud_filtered->points[j].g;
-          b += cloud_filtered->points[j].b;
-      }
-      x = x/clusters[c].indices.size();
-      y = y/clusters[c].indices.size();
-      z = z/clusters[c].indices.size();
-      
-      r = r/clusters[c].indices.size();
-      g = g/clusters[c].indices.size();
-      b = b/clusters[c].indices.size();
-
-      bool new_ = true;
-      // see if we have it detected
-      for( std::vector<Block>::iterator it=marker_names_.begin(); it < marker_names_.end(); it++)
-      {
-        if( (fabs(it->x - x) < block_size*2) &&
-            (fabs(it->y - y) < block_size*2) )
-        {
-          new_ = false;
-          it->active = true;
-          break;
-        }
-      }
-
-      if (new_){
-        // else, add new block
-        addBlock( x, y, z - block_size, 0.0, (float) r/255.0, (float) g/255.0, (float) b/255.0, markers_++ );
-      }
-    }
-
-    // need to delete old blocks
-    for( std::vector<Block>::iterator it=marker_names_.begin(); it < marker_names_.end(); )
-    {
-      if(it->active or it->getName() == last_block_){
-        it++;
-      }else{
-        server.erase( it->getName() );
-        it = marker_names_.erase(it);
-      }
-    }
-
-    server.applyChanges();
-}
-
-};
-
-int main(int argc, char** argv)
-{
-  // initialize node
-  ros::init(argc, argv, "block_manipulation");
-
-  BlockManipulation manip;
-
-  // everything is done in cloud callback, just spin
-  ros::spin();
-}
-

src/turtlebot_arm/turtlebot_arm_block_manipulation/test/block_manipulation_actions.cpp

@@ -1,221 +0,0 @@
-/*
- * Copyright (c) 2011, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- *     * Redistributions of source code must retain the above copyright
- *       notice, this list of conditions and the following disclaimer.
- *     * Redistributions in binary form must reproduce the above copyright
- *       notice, this list of conditions and the following disclaimer in the
- *       documentation and/or other materials provided with the distribution.
- *     * Neither the name of the Willow Garage, Inc. nor the names of its
- *       contributors may be used to endorse or promote products derived from
- *       this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-
-#include <ros/ros.h>
-
-#include <actionlib/client/simple_action_client.h>
-#include <turtlebot_arm_block_manipulation/BlockDetectionAction.h>
-#include <turtlebot_arm_block_manipulation/PickAndPlaceAction.h>
-
-#include <interactive_markers/interactive_marker_server.h>
-
-
-using namespace visualization_msgs;
-
-const std::string arm_link = "/arm_base_link";
-const double gripper_open = 0.042;
-const double gripper_closed = 0.024;
-
-const double z_up = 0.08;
-const double z_down = -0.05;
-
-const double block_size = 0.0127;
-
-
-class BlockManipulationAction
-{
-private:
-  interactive_markers::InteractiveMarkerServer server_;
-  
-  ros::NodeHandle nh_;
-  
-  // Actions
-  actionlib::SimpleActionClient<turtlebot_arm_block_manipulation::BlockDetectionAction> block_detection_action_;
-  actionlib::SimpleActionClient<turtlebot_arm_block_manipulation::PickAndPlaceAction> pick_and_place_action_;
-  
-  turtlebot_arm_block_manipulation::BlockDetectionGoal block_detection_goal_;
-  turtlebot_arm_block_manipulation::PickAndPlaceGoal pick_and_place_goal_;
-  
-  geometry_msgs::Pose old_pose_;
-
-public:
-
-  BlockManipulationAction() : 
-    server_("block_controls"), 
-    block_detection_action_("block_detection", true),
-    pick_and_place_action_("pick_and_place", true)
-  {
-    server_.applyChanges();
-    
-    // Initialize goals
-    block_detection_goal_.frame = arm_link;
-    block_detection_goal_.table_height = z_down;
-    block_detection_goal_.block_size = block_size;
-    
-    pick_and_place_goal_.frame = arm_link;
-    pick_and_place_goal_.z_up = z_up;
-    pick_and_place_goal_.gripper_open = gripper_open;
-    pick_and_place_goal_.gripper_closed = gripper_closed;
-    
-    ROS_INFO("Finished initializing, waiting for servers...");
-    
-    block_detection_action_.waitForServer();
-    pick_and_place_action_.waitForServer();
-    
-    ROS_INFO("Found servers.");
-    
-    detectBlocks();
-  }
-  
-  void detectBlocks()
-  {
-    server_.clear();
-    server_.applyChanges();
-    
-    block_detection_action_.sendGoal(block_detection_goal_,  boost::bind( &BlockManipulationAction::addBlocks, this, _1, _2));
-  }
-  
-  void addBlocks(const actionlib::SimpleClientGoalState& state, const turtlebot_arm_block_manipulation::BlockDetectionResultConstPtr& result)
-  {
-    ROS_INFO("Got block detection callback. Adding blocks.");
-    geometry_msgs::Pose block;
-    
-    for (unsigned int i=0; i < result->blocks.poses.size(); i++)
-    {
-      block = result->blocks.poses[i];
-      addBlock(block.position.x, block.position.y, block.position.z, i);
-      ROS_INFO("Added %d blocks", i);
-    }
-    
-    server_.applyChanges(); 
-  }
-  
-
-  // Move the real block!
-  void feedbackCb( const InteractiveMarkerFeedbackConstPtr &feedback )
-  {
-    switch ( feedback->event_type )
-    {
-      case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN:
-        ROS_INFO_STREAM("Staging " << feedback->marker_name);     
-          old_pose_ = feedback->pose;
-        break;
-   
-      case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP:
-        ROS_INFO_STREAM("Now moving " << feedback->marker_name); 
-        moveBlock(old_pose_, feedback->pose);
-        detectBlocks();
-        break;
-    }
-    
-    server_.applyChanges(); 
-  }
-  
-  bool moveBlock(const geometry_msgs::Pose& start_pose, const geometry_msgs::Pose& end_pose)
-  {
-    pick_and_place_goal_.pickup_pose = start_pose;
-    pick_and_place_goal_.place_pose = end_pose;
-    pick_and_place_action_.sendGoalAndWait(pick_and_place_goal_, ros::Duration(30.0), ros::Duration(30.0));
-    //pick_and_place_action_.sendGoal(pick_and_place_goal_);
-    //pick_and_place_action_.waitForResult(ros::Duration(30.0));
-    
-    if ( block_detection_action_.getState()  == actionlib::SimpleClientGoalState::SUCCEEDED)
-    {
-      return true;
-    }
-    else
-    {
-      return false;
-    }
-  }
-
-  // Make a box
-  Marker makeBox( InteractiveMarker &msg, float r, float g, float b )
-  {
-    Marker m;
-
-    m.type = Marker::CUBE;
-    m.scale.x = msg.scale;
-    m.scale.y = msg.scale;
-    m.scale.z = msg.scale;
-    m.color.r = r;
-    m.color.g = g;
-    m.color.b = b;
-    m.color.a = 1.0;
-
-    return m;
-  }
-   
-  // Add a new block
-  void addBlock( float x, float y, float z, int n)
-  {
-    InteractiveMarker marker;
-    marker.header.frame_id = arm_link;
-    marker.pose.position.x = x;
-    marker.pose.position.y = y;
-    marker.pose.position.z = z;
-    marker.scale = 0.03;
-    
-    std::stringstream conv;
-    conv << n;
-    conv.str();
-     
-    marker.name = "block" + conv.str();
-
-    InteractiveMarkerControl control;
-    control.orientation.w = 1;
-    control.orientation.x = 0;
-    control.orientation.y = 1;
-    control.orientation.z = 0;
-    control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
-    marker.controls.push_back( control );
-    
-    control.markers.push_back( makeBox(marker, .5, .5, .5) );
-    control.always_visible = true;
-    marker.controls.push_back( control );
-    
-
-    server_.insert( marker );
-    server_.setCallback( marker.name, boost::bind( &BlockManipulationAction::feedbackCb, this, _1 ));
-  }
-
-};
-
-int main(int argc, char** argv)
-{
-  // initialize node
-  ros::init(argc, argv, "block_manipulation");
-
-  BlockManipulationAction manip;
-
-  // everything is done in cloud callback, just spin
-  ros::spin();
-}
-

src/turtlebot_arm/turtlebot_arm_bringup/.cproject

@@ -1,42 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" standalone="no"?>
-<?fileVersion 4.0.0?><cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
-	<storageModule moduleId="org.eclipse.cdt.core.settings">
-		<cconfiguration id="cdt.managedbuild.toolchain.gnu.base.515139767">
-			<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.base.515139767" moduleId="org.eclipse.cdt.core.settings" name="Default">
-				<externalSettings/>
-				<extensions>
-					<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
-				</extensions>
-			</storageModule>
-			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
-				<configuration buildProperties="" id="cdt.managedbuild.toolchain.gnu.base.515139767" name="Default" parent="org.eclipse.cdt.build.core.emptycfg">
-					<folderInfo id="cdt.managedbuild.toolchain.gnu.base.515139767.827115284" name="/" resourcePath="">
-						<toolChain id="cdt.managedbuild.toolchain.gnu.base.1608674226" name="Linux GCC" superClass="cdt.managedbuild.toolchain.gnu.base">
-							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="cdt.managedbuild.target.gnu.platform.base.127218381" name="Debug Platform" osList="linux,hpux,aix,qnx" superClass="cdt.managedbuild.target.gnu.platform.base"/>
-							<builder id="cdt.managedbuild.target.gnu.builder.base.255394953" managedBuildOn="false" name="Gnu Make Builder.Default" superClass="cdt.managedbuild.target.gnu.builder.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.archiver.base.1703134418" name="GCC Archiver" superClass="cdt.managedbuild.tool.gnu.archiver.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.cpp.compiler.base.419427586" name="GCC C++ Compiler" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.c.compiler.base.2126916129" name="GCC C Compiler" superClass="cdt.managedbuild.tool.gnu.c.compiler.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.c.linker.base.1491317168" name="GCC C Linker" superClass="cdt.managedbuild.tool.gnu.c.linker.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.cpp.linker.base.1637706310" name="GCC C++ Linker" superClass="cdt.managedbuild.tool.gnu.cpp.linker.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.assembler.base.1115661047" name="GCC Assembler" superClass="cdt.managedbuild.tool.gnu.assembler.base"/>
-						</toolChain>
-					</folderInfo>
-				</configuration>
-			</storageModule>
-			<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
-		</cconfiguration>
-	</storageModule>
-	<storageModule moduleId="cdtBuildSystem" version="4.0.0">
-		<project id="turtlebot_arm_bringup.null.1254331987" name="turtlebot_arm_bringup"/>
-	</storageModule>
-	<storageModule moduleId="scannerConfiguration">
-		<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
-	</storageModule>
-	<storageModule moduleId="org.eclipse.cdt.core.LanguageSettingsProviders"/>
-</cproject>

src/turtlebot_arm/turtlebot_arm_bringup/.project

@@ -1,33 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<projectDescription>
-	<name>turtlebot_arm_bringup</name>
-	<comment></comment>
-	<projects>
-	</projects>
-	<buildSpec>
-		<buildCommand>
-			<name>org.python.pydev.PyDevBuilder</name>
-			<arguments>
-			</arguments>
-		</buildCommand>
-		<buildCommand>
-			<name>org.eclipse.cdt.managedbuilder.core.genmakebuilder</name>
-			<triggers>clean,full,incremental,</triggers>
-			<arguments>
-			</arguments>
-		</buildCommand>
-		<buildCommand>
-			<name>org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder</name>
-			<triggers>full,incremental,</triggers>
-			<arguments>
-			</arguments>
-		</buildCommand>
-	</buildSpec>
-	<natures>
-		<nature>org.eclipse.cdt.core.cnature</nature>
-		<nature>org.eclipse.cdt.core.ccnature</nature>
-		<nature>org.eclipse.cdt.managedbuilder.core.managedBuildNature</nature>
-		<nature>org.eclipse.cdt.managedbuilder.core.ScannerConfigNature</nature>
-		<nature>org.python.pydev.pythonNature</nature>
-	</natures>
-</projectDescription>

src/turtlebot_arm/turtlebot_arm_bringup/.pydevproject

@@ -1,5 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" standalone="no"?>
-<?eclipse-pydev version="1.0"?><pydev_project>
-<pydev_property name="org.python.pydev.PYTHON_PROJECT_INTERPRETER">Default</pydev_property>
-<pydev_property name="org.python.pydev.PYTHON_PROJECT_VERSION">python 2.7</pydev_property>
-</pydev_project>

src/turtlebot_arm/turtlebot_arm_bringup/CHANGELOG.rst

@@ -1,17 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package turtlebot_arm_bringup
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-0.3.3 (2014-09-20)
-------------------
-
-0.3.2 (2014-08-30)
-------------------
-* Use gripper_controller instead of gripper_controller.py, as the later doesn't get installed
-
-0.3.1 (2014-08-22)
-------------------
-
-0.3.0 (2014-08-16)
-------------------
-* First indigo release

src/turtlebot_arm/turtlebot_arm_bringup/CMakeLists.txt

@@ -1,13 +0,0 @@
-cmake_minimum_required(VERSION 2.8.3)
-project(turtlebot_arm_bringup)
-
-find_package(catkin REQUIRED)
-catkin_package()
-
-install(DIRECTORY config
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-
-install(DIRECTORY launch
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)

src/turtlebot_arm/turtlebot_arm_bringup/config/turtlebot_arm.yaml

@@ -1,14 +0,0 @@
-source: pincher_arm.yaml
-port: /dev/ttyUSB0
-read_rate: 15
-write_rate: 25
-joints: {
-    arm_shoulder_pan_joint: {id: 1,  neutral: 512, max_angle: 140, min_angle: -140, max_speed: 90, type: dynamixel},
-    arm_shoulder_lift_joint: {id: 2, max_angle: 126, min_angle: -119, max_speed: 90, type: dynamixel},
-    arm_elbow_flex_joint: {id: 3, max_angle: 136, min_angle: -139, max_speed: 90, type: dynamixel},
-    arm_wrist_flex_joint: {id: 4, max_angle: 96, min_angle: -98, max_speed: 90, type: dynamixel},
-    gripper_joint: {id: 5, max_angle: 0, min_angle: -125, max_speed: 90, type: dynamixel}
-}
-controllers: {
-    arm_controller: {type: follow_controller, joints: [arm_shoulder_pan_joint, arm_shoulder_lift_joint, arm_elbow_flex_joint, arm_wrist_flex_joint], action_name: arm_controller/follow_joint_trajectory, onboard: False }
-}

src/turtlebot_arm/turtlebot_arm_bringup/config/turtlebot_gripper.yaml

@@ -1,10 +0,0 @@
-source: turtlebot_gripper.yaml
-model:  parallel
-invert: false
-pad_width: 0.002
-min_opening: 0.002
-max_opening: 1.031
-neutral: 0.015
-tighten: .001
-center: 0
-joint: gripper_joint

src/turtlebot_arm/turtlebot_arm_bringup/launch/arm.launch

@@ -1,17 +0,0 @@
-<launch>
-  <arg name="simulation" default="false"/>
-  <arg name="arm_type" default="turtlebot" />
-
-  <param name="robot_description" command="$(find xacro)/xacro '$(find turtlebot_arm_description)/urdf/$(arg arm_type)_arm.urdf.xacro'"/>
-  <node name="robot_state_pub" pkg="robot_state_publisher" type="robot_state_publisher"/>
-  <node name="fake_joint_pub" pkg="turtlebot_arm_bringup" type="fake_joint_pub.py"/>
-
-  <node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
-    <rosparam file="$(find turtlebot_arm_bringup)/config/$(arg arm_type)_arm.yaml" command="load"/>
-    <param name="sim" value="$(arg simulation)"/>
-  </node>
-
-  <node name="gripper_controller" pkg="arbotix_controllers" type="gripper_controller" output="screen">
-    <rosparam file="$(find turtlebot_arm_bringup)/config/$(arg arm_type)_gripper.yaml" command="load" />
-  </node>
-</launch>

src/turtlebot_arm/turtlebot_arm_bringup/package.xml

@@ -1,19 +0,0 @@
-<package>
-  <name>turtlebot_arm_bringup</name>
-  <version>0.3.3</version>
-  <description>
-   turtlebot_arm_bringup provides launch files for starting the drivers for the TurtleBot arm.
-  </description>
-  <author>Michael Ferguson</author>
-  <author>Helen Oleynikova</author>
-  <author>Melonee Wise</author>
-  <maintainer email="jsantossimon@gmail.com">Jorge Santos</maintainer>
-  <license>BSD</license>
-  
-  <url>http://ros.org/wiki/turtlebot_arm_bringup</url>
-
-  <buildtool_depend>catkin</buildtool_depend>
-
-  <run_depend>arbotix_python</run_depend>
-  <run_depend>arbotix_controllers</run_depend>
-</package>

src/turtlebot_arm/turtlebot_arm_bringup/scripts/fake_joint_pub.py

@@ -1,18 +0,0 @@
-#!/usr/bin/env python
-
-import rospy
-from sensor_msgs.msg import JointState
-
-rospy.init_node("fake_joint_pub")
-p = rospy.Publisher('joint_states', JointState, queue_size=5)
-
-msg = JointState()
-msg.name = ["gripper_link_joint"]
-msg.position = [0.0 for name in msg.name]
-msg.velocity = [0.0 for name in msg.name]
-
-while not rospy.is_shutdown():
-    msg.header.stamp = rospy.Time.now()
-    p.publish(msg)
-    rospy.sleep(0.05)
-

src/turtlebot_arm/turtlebot_arm_description/.cproject

@@ -1,42 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" standalone="no"?>
-<?fileVersion 4.0.0?><cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
-	<storageModule moduleId="org.eclipse.cdt.core.settings">
-		<cconfiguration id="cdt.managedbuild.toolchain.gnu.base.57332903">
-			<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.base.57332903" moduleId="org.eclipse.cdt.core.settings" name="Default">
-				<externalSettings/>
-				<extensions>
-					<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
-				</extensions>
-			</storageModule>
-			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
-				<configuration buildProperties="" id="cdt.managedbuild.toolchain.gnu.base.57332903" name="Default" parent="org.eclipse.cdt.build.core.emptycfg">
-					<folderInfo id="cdt.managedbuild.toolchain.gnu.base.57332903.1744757409" name="/" resourcePath="">
-						<toolChain id="cdt.managedbuild.toolchain.gnu.base.1868380160" name="Linux GCC" superClass="cdt.managedbuild.toolchain.gnu.base">
-							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="cdt.managedbuild.target.gnu.platform.base.509388424" name="Debug Platform" osList="linux,hpux,aix,qnx" superClass="cdt.managedbuild.target.gnu.platform.base"/>
-							<builder id="cdt.managedbuild.target.gnu.builder.base.1621856178" managedBuildOn="false" name="Gnu Make Builder.Default" superClass="cdt.managedbuild.target.gnu.builder.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.archiver.base.2024210912" name="GCC Archiver" superClass="cdt.managedbuild.tool.gnu.archiver.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.cpp.compiler.base.783752217" name="GCC C++ Compiler" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.c.compiler.base.1949400212" name="GCC C Compiler" superClass="cdt.managedbuild.tool.gnu.c.compiler.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.c.linker.base.789737065" name="GCC C Linker" superClass="cdt.managedbuild.tool.gnu.c.linker.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.cpp.linker.base.2050862217" name="GCC C++ Linker" superClass="cdt.managedbuild.tool.gnu.cpp.linker.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.assembler.base.1445781697" name="GCC Assembler" superClass="cdt.managedbuild.tool.gnu.assembler.base"/>
-						</toolChain>
-					</folderInfo>
-				</configuration>
-			</storageModule>
-			<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
-		</cconfiguration>
-	</storageModule>
-	<storageModule moduleId="cdtBuildSystem" version="4.0.0">
-		<project id="turtlebot_arm_description.null.320341852" name="turtlebot_arm_description"/>
-	</storageModule>
-	<storageModule moduleId="scannerConfiguration">
-		<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
-	</storageModule>
-	<storageModule moduleId="org.eclipse.cdt.core.LanguageSettingsProviders"/>
-</cproject>

src/turtlebot_arm/turtlebot_arm_description/.project

@@ -1,27 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<projectDescription>
-	<name>turtlebot_arm_description</name>
-	<comment></comment>
-	<projects>
-	</projects>
-	<buildSpec>
-		<buildCommand>
-			<name>org.eclipse.cdt.managedbuilder.core.genmakebuilder</name>
-			<triggers>clean,full,incremental,</triggers>
-			<arguments>
-			</arguments>
-		</buildCommand>
-		<buildCommand>
-			<name>org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder</name>
-			<triggers>full,incremental,</triggers>
-			<arguments>
-			</arguments>
-		</buildCommand>
-	</buildSpec>
-	<natures>
-		<nature>org.eclipse.cdt.core.cnature</nature>
-		<nature>org.eclipse.cdt.core.ccnature</nature>
-		<nature>org.eclipse.cdt.managedbuilder.core.managedBuildNature</nature>
-		<nature>org.eclipse.cdt.managedbuilder.core.ScannerConfigNature</nature>
-	</natures>
-</projectDescription>

src/turtlebot_arm/turtlebot_arm_description/CHANGELOG.rst

@@ -1,18 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package turtlebot_arm_description
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-0.3.3 (2014-09-20)
-------------------
-
-0.3.2 (2014-08-30)
-------------------
-* Use base_link instead of base_footprint as the reference "fixed" frame
-
-0.3.1 (2014-08-22)
-------------------
-* Set separated URLs for website, repository and bugtracker
-
-0.3.0 (2014-08-16)
-------------------
-* First indigo release

src/turtlebot_arm/turtlebot_arm_description/CMakeLists.txt

@@ -1,13 +0,0 @@
-cmake_minimum_required(VERSION 2.8.3)
-project(turtlebot_arm_description)
-
-find_package(catkin REQUIRED)
-catkin_package()
-
-install(DIRECTORY meshes
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-
-install(DIRECTORY urdf
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)

src/turtlebot_arm/turtlebot_arm_description/meshes/README

@@ -1,3 +0,0 @@
-The meshes for these parts were created by I-Bioloid and obtain from:
-http://www.thingiverse.com/thing:5192
-

src/turtlebot_arm/turtlebot_arm_description/package.xml

@@ -1,20 +0,0 @@
-<package>
-  <name>turtlebot_arm_description</name>
-  <version>0.3.3</version>
-  <description>
-   turtlebot_arm_description contains URDF files and meshes for the TurtleBot arm.
-  </description>
-  <author>Michael Ferguson</author>
-  <author>Melonee Wise</author>
-  <maintainer email="jsantossimon@gmail.com">Jorge Santos</maintainer>
-  <license>BSD</license>
-  
-  <url type="website">http://ros.org/wiki/turtlebot_arm_description</url>
-  <url type="repository">https://github.com/turtlebot/turtlebot_arm</url>
-  <url type="bugtracker">https://github.com/turtlebot/turtlebot_arm/issues</url>
-
-  <buildtool_depend>catkin</buildtool_depend>
-
-  <run_depend>urdf</run_depend>
-  <run_depend>xacro</run_depend>
-</package>

src/turtlebot_arm/turtlebot_arm_description/test.launch

@@ -1,18 +0,0 @@
-<launch>
-  <!-- To use, first set Environment variable TURTLEBOT_ARM1 to either:
-   turtlebot or pincher (for Trossen PhantomX Pincher)
-   NOTE: passing arm_type as argument NOT yet fully supported! -->
-
-  <arg name="simulation" default="false"/>
-  <arg name="arm_type" default="$(optenv TURTLEBOT_ARM1 turtlebot)"/>
-
-  <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find turtlebot_arm_description)/urdf/$(arg arm_type)_arm.urdf.xacro'"/>
-
-  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
-    <param name="use_gui" value="True"/>
-  </node>
-
-  <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
-    <param name="publish_frequency" type="double" value="10.0"/>
-  </node>
-</launch>

src/turtlebot_arm/turtlebot_arm_description/urdf/arm_hardware.xacro

@@ -1,361 +0,0 @@
-<?xml version="1.0"?>
-<!-- Basic URDF components for Turtlebot Arm and PhantomX Pincher Arm-->
-<robot xmlns:xacro="http://ros.org/wiki/xacro">
-  <xacro:property name="M_PI" value="3.14159"/>
-  <xacro:property name="M_SCALE" value="0.001"/>
-  <xacro:property name="F10_HEIGHT" value="0.004"/>
-  <xacro:property name="F4_HEIGHT" value="0.0525"/>
-  <xacro:property name="F3_HEIGHT" value="0.009"/>
-  <xacro:property name="AX12_HEIGHT" value="0.0385"/>
-  <xacro:property name="AX12_WIDTH" value="0.038"/>
-  <xacro:property name="AX12_LENGTH" value="0.050"/>
-  <xacro:property name="F2_HEIGHT" value="0.0265"/>
-
-  <!-- Calculates the inertial properties of a bounding box. From
-       https://en.wikipedia.org/wiki/List_of_moments_of_inertia#List_of_3D_inertia_tensors-->
-  <xacro:macro name="box_inertia" params="length width height mass">
-    <inertial>
-      <mass value="${mass}"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="${mass/12*(height**2+length**2)}" ixy="0" ixz = "0"
-               iyy="${mass/12*(width**2+length**2)}" iyz="0"
-               izz="${mass/12*(width**2+height**2)}"/>
-    </inertial>
-  </xacro:macro>
-
-  <xacro:macro name="arm_mount" params="parent name color *origin">
-    <joint name="${name}_joint" type="fixed">
-      <xacro:insert_block name="origin"/>
-      <parent link="${parent}"/>
-      <child link="${name}_link"/>
-    </joint>
-
-    <link name="${name}_link">
-      <xacro:box_inertia length="0.074" width="0.074" height="0.004" mass="0.018"/>
-
-      <visual>
-        <origin xyz="-0.028 -0.0493 0.014" rpy="0.0 0.0 -${M_PI/2}"/>
-        <geometry>
-          <mesh filename="package://turtlebot_arm_description/meshes/mount.stl" scale="${M_SCALE} ${M_SCALE} ${M_SCALE}"/>
-        </geometry>
-        <material name="${color}"/>
-      </visual>
-
-      <collision>
-        <origin xyz="-0.01 0.0 0.0165" rpy="0.0 0.0 0.0"/>
-        <geometry>
-          <box size="0.074 0.074 0.004"/>
-        </geometry>
-      </collision>
-    </link>
-    
-    <gazebo reference="${name}_link">
-      <material>Gazebo/${color}</material>
-      <selfCollide>true</selfCollide>
-      <gravity>true</gravity>
-    </gazebo>
-  </xacro:macro>
-
-  <xacro:macro name="finger_fixed" params="parent name color *origin">
-    <joint name="${name}_joint" type="fixed">
-      <xacro:insert_block name="origin"/>
-      <parent link="${parent}"/>
-      <child link="${name}_link"/>
-    </joint>
-
-    <link name="${name}_link">
-      <xacro:box_inertia length="0.0783" width="0.03801" height="0.0193" mass="0.015"/>
-
-      <visual>
-        <origin xyz=" 0 0 0 " rpy="0 0 0"/>
-        <geometry>
-          <mesh filename="package://turtlebot_arm_description/meshes/turtlebot_finger.stl" scale="${M_SCALE} ${M_SCALE} ${M_SCALE}"/>
-        </geometry>
-        <material name="${color}"/>
-      </visual>
-
-      <collision>
-        <origin xyz="0.02645 0.0 -0.00655" rpy="0 0 0"/>
-        <geometry>
-          <box size="0.0783 0.03801 0.0193"/>
-        </geometry>
-      </collision>
-    </link>
-    
-    <gazebo reference="${name}_link">
-      <material>Gazebo/${color}</material>
-      <selfCollide>true</selfCollide>
-      <gravity>true</gravity>
-    </gazebo>
-  </xacro:macro>
-
-  <xacro:macro name="dynamixel_AX12_fixed" params="parent name *origin">
-    <joint name="${name}_joint" type="fixed">
-      <xacro:insert_block name="origin"/>
-      <parent link="${parent}"/>
-      <child link="${name}_link"/>
-    </joint>
-
-    <link name="${name}_link">
-      <inertial>
-        <mass value="0.055"/>
-        <origin xyz="0 0 0"/>
-        <inertia ixx="0.000017012" ixy="0.0" ixz="0.0"
-                 iyy="0.000013258" iyz="0.0"
-                 izz="0.000009483"/>
-      </inertial>
-
-      <visual>
-        <origin xyz=" 0 0 0 " rpy="0 0 0"/>
-        <geometry>
-          <mesh filename="package://turtlebot_arm_description/meshes/ax12_box.stl" scale="${M_SCALE} ${M_SCALE} ${M_SCALE}"/>
-        </geometry>
-        <material name="Black"/>
-      </visual>
-
-      <collision>
-        <origin xyz="0.0 0.0 -0.01241" rpy="0 0 0"/>
-        <geometry>
-          <box size="0.025 0.038 0.04762"/>
-        </geometry>
-      </collision>
-    </link>
-    
-    <gazebo reference="${name}_link">
-      <material>Gazebo/Black</material>
-      <selfCollide>true</selfCollide>
-      <gravity>true</gravity>
-    </gazebo>
-  </xacro:macro>
-
-  <xacro:macro name="bioloid_F10_fixed" params="parent name color *origin">
-    <joint name="${name}_joint" type="fixed">
-      <xacro:insert_block name="origin"/>
-      <parent link="${parent}"/>
-      <child link="${name}_link"/>
-    </joint>
-
-    <link name="${name}_link">
-      <xacro:box_inertia length="0.025" width="0.038" height="0.004" mass="0.004"/>
-
-      <visual>
-        <origin xyz=" 0 0 0 " rpy="0 0 0"/>
-        <geometry>
-          <mesh filename="package://turtlebot_arm_description/meshes/F10.stl" scale="${M_SCALE} ${M_SCALE} ${M_SCALE}"/>
-        </geometry>
-        <material name="${color}"/>
-      </visual>
-
-      <collision>
-        <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
-        <geometry>
-          <box size="0.025 0.038 0.004"/>
-        </geometry>
-      </collision>
-    </link>
-    
-    <gazebo reference="${name}_link">
-      <material>Gazebo/${color}</material>
-      <selfCollide>true</selfCollide>
-      <gravity>true</gravity>
-    </gazebo>
-  </xacro:macro>
-
-  <xacro:macro name="bioloid_F3_fixed" params="parent name color *origin">
-    <joint name="${name}_joint" type="fixed">
-      <xacro:insert_block name="origin"/>
-      <parent link="${parent}"/>
-      <child link="${name}_link"/>
-    </joint>
-
-    <link name="${name}_link">
-      <xacro:box_inertia length="0.025" width="0.038" height="0.009" mass="0.005"/>
-
-      <visual>
-        <origin xyz=" 0 0 0 " rpy="0 0 0"/>
-        <geometry>
-          <mesh filename="package://turtlebot_arm_description/meshes/F3.stl" scale="${M_SCALE} ${M_SCALE} ${M_SCALE}"/>
-        </geometry>
-        <material name="${color}"/>
-      </visual>
-
-      <collision>
-        <origin xyz="0.0 0.0 -0.0045" rpy="0 0 0"/>
-        <geometry>
-          <box size="0.025 0.038 0.009"/>
-        </geometry>
-      </collision>
-    </link>
-    
-    <gazebo reference="${name}_link">
-      <material>Gazebo/${color}</material>
-      <selfCollide>true</selfCollide>
-      <gravity>true</gravity>
-    </gazebo>
-  </xacro:macro>
-
-  <xacro:macro name="bioloid_F3_revolute" params="parent name color llimit ulimit vlimit *origin">
-    <joint name="${name}_joint" type="revolute">
-      <xacro:insert_block name="origin"/>
-      <axis xyz="0 0 -1"/>
-      <limit effort="30" velocity="${vlimit}" lower="${llimit}" upper="${ulimit}"/>
-      <dynamics friction="0.13"/>
-      <parent link="${parent}"/>
-      <child link="${name}_link"/>
-    </joint>
-
-    <link name="${name}_link">
-      <xacro:box_inertia length="0.025" width="0.038" height="0.009" mass="0.008"/>
-
-      <visual>
-        <origin xyz=" 0 0 0 " rpy="0 0 0"/>
-        <geometry>
-          <mesh filename="package://turtlebot_arm_description/meshes/F3.stl" scale="${M_SCALE} ${M_SCALE} ${M_SCALE}"/>
-        </geometry>
-        <material name="${color}"/>
-      </visual>
-
-      <collision>
-        <origin xyz="0.0 0.0 -0.0045" rpy="0 0 0"/>
-        <geometry>
-          <box size="0.025 0.038 0.009"/>
-        </geometry>
-      </collision>
-    </link>
-    
-    <gazebo reference="${name}_link">
-      <material>Gazebo/${color}</material>
-      <selfCollide>true</selfCollide>
-      <gravity>true</gravity>
-    </gazebo>
-  </xacro:macro>
-
-  <xacro:macro name="bioloid_F2_revolute" params="parent name color llimit ulimit vlimit *origin">
-    <joint name="${name}_joint" type="revolute">
-      <xacro:insert_block name="origin"/>
-      <axis xyz="0 1 0"/>
-      <limit effort="30" velocity="${vlimit}" lower="${llimit}" upper="${ulimit}"/>
-      <dynamics friction="0.13"/>
-      <parent link="${parent}"/>
-      <child link="${name}_link"/>
-    </joint>
-
-    <link name="${name}_link">
-      <xacro:box_inertia length="0.025" width="0.0485" height="0.0375" mass="0.010"/>
-
-      <visual>
-        <origin xyz=" 0 0 0 " rpy="0 0 0"/>
-        <geometry>
-          <mesh filename="package://turtlebot_arm_description/meshes/F2.stl" scale="${M_SCALE} ${M_SCALE} ${M_SCALE}"/>
-        </geometry>
-        <material name="${color}"/>
-      </visual>
-
-      <collision>
-        <origin xyz="0.0 0.0 0.00775" rpy="0 0 0"/>
-        <geometry>
-          <box size="0.025 0.0485 0.0375"/>
-        </geometry>
-      </collision>
-    </link>
-    
-    <gazebo reference="${name}_link">
-      <material>Gazebo/${color}</material>
-      <selfCollide>true</selfCollide>
-      <gravity>true</gravity>
-    </gazebo>
-  </xacro:macro>
-
-  <xacro:macro name="bioloid_F4_revolute" params="parent name color llimit ulimit vlimit *origin">
-    <joint name="${name}_joint" type="revolute">
-      <xacro:insert_block name="origin"/>
-      <axis xyz="0 1 0"/>
-      <limit effort="30" velocity="${vlimit}" lower="${llimit}" upper="${ulimit}"/>
-      <dynamics friction="0.13"/>
-      <parent link="${parent}"/>
-      <child link="${name}_link"/>
-    </joint>
-
-    <link name="${name}_link">
-      <xacro:box_inertia length="0.028" width="0.0485" height="0.065" mass="0.016"/>
-
-      <visual>
-        <origin xyz=" 0 0 0 " rpy="0 0 0"/>
-        <geometry>
-          <mesh filename="package://turtlebot_arm_description/meshes/F4.stl" scale="${M_SCALE} ${M_SCALE} ${M_SCALE}"/>
-        </geometry>
-        <material name="${color}"/>
-      </visual>
-
-      <collision>
-        <origin xyz="0.0 0.0 0.0215" rpy="0 0 0"/>
-        <geometry>
-          <box size="0.028 0.0485 0.065"/>
-        </geometry>
-      </collision>
-    </link>
-    
-    <gazebo reference="${name}_link">
-      <material>Gazebo/${color}</material>
-      <selfCollide>true</selfCollide>
-      <gravity>true</gravity>
-    </gazebo>
-  </xacro:macro>
-  
-  <xacro:macro name="gripper_finger_base" params="parent name color *origin">
-    <joint name="${name}_joint" type="fixed">
-      <xacro:insert_block name="origin"/>
-      <parent link="${parent}"/>
-      <child link="${name}_link"/>
-    </joint>
-    
-    <link name="${name}_link">
-      <xacro:box_inertia length="0.002" width="0.040" height="0.075" mass="0.00001"/>
-      <visual>
-        <origin xyz="0.016 0 -.015 " rpy="${M_PI} ${-M_PI/2} ${M_PI/2}"/>
-        <geometry>
-          <mesh filename="package://turtlebot_arm_description/meshes/pincher_finger_base.stl" scale=".01 .01 .01"/>
-        </geometry>
-        <material name="${color}"/>
-      </visual>
-      
-      <collision>
-        <origin xyz="0.016 0.0 -0.015" rpy="0 0 0"/>
-        <geometry>
-          <box size="0.002 0.040 0.075"/>
-        </geometry>
-      </collision>
-    </link >
-    
-    <gazebo reference="${name}_link">
-      <material>Gazebo/${color}</material>
-      <selfCollide>true</selfCollide>
-      <gravity>true</gravity>
-    </gazebo>
-  </xacro:macro>
-  
-    <xacro:macro name="pincher_gripper" params="name color *origin">
-      <link name="${name}_link">
-      <xacro:box_inertia length="0.036" width="0.028" height="0.004" mass="0.00001"/>
-      <visual>
-        <xacro:insert_block name="origin"/>
-        <geometry>
-          <mesh filename="package://turtlebot_arm_description/meshes/pincher_finger.stl" scale=".01 .01 .01"/>
-        </geometry>
-        <material name="${color}"/>
-      </visual>
-      <collision>
-        <xacro:insert_block name="origin"/>
-        <geometry>
-          <box size="0.023 0.028 0.038"/>
-        </geometry>
-      </collision>
-    </link>
-  
-    <gazebo reference="${name}_link">
-      <material>Gazebo/${color}</material>
-      <selfCollide>true</selfCollide>
-      <gravity>true</gravity>
-    </gazebo>
-  </xacro:macro>
-</robot>

src/turtlebot_arm/turtlebot_arm_description/urdf/pincher_arm.urdf

@@ -1,697 +0,0 @@
-<?xml version="1.0" ?>
-<!-- =================================================================================== -->
-<!-- |    This document was autogenerated by xacro from pincher_arm.urdf.xacro         | -->
-<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
-<!-- =================================================================================== -->
-<!-- Describe URDF for PhantomX Pincher Arm -->
-<robot name="turtlebot_arm" xmlns:xacro="http://ros.org/wiki/xacro">
-  <!-- Included URDF Files -->
-  <material name="White">
-    <color rgba="0.87 0.90 0.87 1.0"/>
-  </material>
-  <material name="Black">
-    <color rgba="0.08 0.08 0.08 1.0"/>
-  </material>
-  <material name="Gray">
-    <color rgba="0.2 0.2 0.21 1.0"/>
-  </material>
-  <material name="Green">
-    <color rgba="0.22 0.32 0.14 1.0"/>
-  </material>
-  <!-- As we don't have here a turtlebot base, add a base_link link as its location reference -->
-  <link name="base_link"/>
-  <link name="arm_base_link"/>
-  <joint name="arm_base_joint" type="fixed">
-    <origin xyz="0.1 0.03 0.435"/>
-    <parent link="base_link"/>
-    <child link="arm_base_link"/>
-  </joint>
-  <!-- fake gripper_link joint gives us a free servo!
-         this makes us 5DOF and saves you $44.90
-         that's a lot of coin! -->
-  <link name="gripper_link"/>
-  <joint name="gripper_link_joint" type="revolute">
-    <origin rpy="0 -1.57 0" xyz="0 0 0.112"/>
-    <parent link="arm_wrist_flex_link"/>
-    <child link="gripper_link"/>
-    <limit effort="30" lower="-3.14" upper="3.14" velocity="1"/>
-    <axis xyz="1 0 0"/>
-  </joint>
-  <joint name="arm_shoulder_pan_servo_joint" type="fixed">
-    <origin rpy="1.570795 0 1.570795" xyz="0 0 0"/>
-    <parent link="arm_base_link"/>
-    <child link="arm_shoulder_pan_servo_link"/>
-  </joint>
-  <link name="arm_shoulder_pan_servo_link">
-    <inertial>
-      <mass value="0.055"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="0.000017012" ixy="0.0" ixz="0.0" iyy="0.000013258" iyz="0.0" izz="0.000009483"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/ax12_box.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="Black"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 -0.01241"/>
-      <geometry>
-        <box size="0.025 0.038 0.04762"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="arm_shoulder_pan_servo_link">
-    <material>Gazebo/Black</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <joint name="arm_shoulder_pan_joint" type="revolute">
-    <origin rpy="-1.570795 1.570795 3.14159" xyz="0 0.019 0"/>
-    <axis xyz="0 0 -1"/>
-    <limit effort="30" lower="-2.617" upper="2.617" velocity="1.571"/>
-    <dynamics friction="0.13"/>
-    <parent link="arm_shoulder_pan_servo_link"/>
-    <child link="arm_shoulder_pan_link"/>
-  </joint>
-  <link name="arm_shoulder_pan_link">
-    <inertial>
-      <mass value="0.008"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="4.70666666667e-07" ixy="0" ixz="0" iyy="1.37933333333e-06" iyz="0" izz="1.01666666667e-06"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/F3.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="Gray"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 -0.0045"/>
-      <geometry>
-        <box size="0.025 0.038 0.009"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="arm_shoulder_pan_link">
-    <material>Gazebo/Gray</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <joint name="arm_shoulder_lift_servo_joint" type="fixed">
-    <origin rpy="0 3.14159 0" xyz="0 0 -0.0415"/>
-    <parent link="arm_shoulder_pan_link"/>
-    <child link="arm_shoulder_lift_servo_link"/>
-  </joint>
-  <link name="arm_shoulder_lift_servo_link">
-    <inertial>
-      <mass value="0.055"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="0.000017012" ixy="0.0" ixz="0.0" iyy="0.000013258" iyz="0.0" izz="0.000009483"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/ax12_box.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="Black"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 -0.01241"/>
-      <geometry>
-        <box size="0.025 0.038 0.04762"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="arm_shoulder_lift_servo_link">
-    <material>Gazebo/Black</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <joint name="arm_shoulder_lift_joint" type="revolute">
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-    <axis xyz="0 1 0"/>
-    <limit effort="30" lower="-2.2" upper="2.16" velocity="1.571"/>
-    <dynamics friction="0.13"/>
-    <parent link="arm_shoulder_lift_servo_link"/>
-    <child link="arm_shoulder_lift_link"/>
-  </joint>
-  <link name="arm_shoulder_lift_link">
-    <inertial>
-      <mass value="0.016"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="6.67866666667e-06" ixy="0" ixz="0" iyy="4.18166666667e-06" iyz="0" izz="8.76966666667e-06"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/F4.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="Gray"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.0215"/>
-      <geometry>
-        <box size="0.028 0.0485 0.065"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="arm_shoulder_lift_link">
-    <material>Gazebo/Gray</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <joint name="arm_shoulder_F10_0_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0.0545"/>
-    <parent link="arm_shoulder_lift_link"/>
-    <child link="arm_shoulder_F10_0_link"/>
-  </joint>
-  <link name="arm_shoulder_F10_0_link">
-    <inertial>
-      <mass value="0.004"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="2.13666666667e-07" ixy="0" ixz="0" iyy="6.89666666667e-07" iyz="0" izz="4.86666666667e-07"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/F10.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="Gray"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-      <geometry>
-        <box size="0.025 0.038 0.004"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="arm_shoulder_F10_0_link">
-    <material>Gazebo/Gray</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <joint name="arm_shoulder_F10_1_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0.004"/>
-    <parent link="arm_shoulder_F10_0_link"/>
-    <child link="arm_shoulder_F10_1_link"/>
-  </joint>
-  <link name="arm_shoulder_F10_1_link">
-    <inertial>
-      <mass value="0.004"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="2.13666666667e-07" ixy="0" ixz="0" iyy="6.89666666667e-07" iyz="0" izz="4.86666666667e-07"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/F10.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="Gray"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-      <geometry>
-        <box size="0.025 0.038 0.004"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="arm_shoulder_F10_1_link">
-    <material>Gazebo/Gray</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <joint name="arm_shoulder_F10_2_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0.004"/>
-    <parent link="arm_shoulder_F10_1_link"/>
-    <child link="arm_shoulder_F10_2_link"/>
-  </joint>
-  <link name="arm_shoulder_F10_2_link">
-    <inertial>
-      <mass value="0.004"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="2.13666666667e-07" ixy="0" ixz="0" iyy="6.89666666667e-07" iyz="0" izz="4.86666666667e-07"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/F10.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="Gray"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-      <geometry>
-        <box size="0.025 0.038 0.004"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="arm_shoulder_F10_2_link">
-    <material>Gazebo/Gray</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <joint name="arm_shoulder_F3_0_joint" type="fixed">
-    <origin rpy="0 3.14159 0" xyz="0 0 0.002"/>
-    <parent link="arm_shoulder_F10_2_link"/>
-    <child link="arm_shoulder_F3_0_link"/>
-  </joint>
-  <link name="arm_shoulder_F3_0_link">
-    <inertial>
-      <mass value="0.005"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="2.94166666667e-07" ixy="0" ixz="0" iyy="8.62083333333e-07" iyz="0" izz="6.35416666667e-07"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/F3.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="Gray"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 -0.0045"/>
-      <geometry>
-        <box size="0.025 0.038 0.009"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="arm_shoulder_F3_0_link">
-    <material>Gazebo/Gray</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <joint name="arm_elbow_flex_servo_joint" type="fixed">
-    <origin rpy="0 3.14159 0" xyz="0 0 -0.0415"/>
-    <parent link="arm_shoulder_F3_0_link"/>
-    <child link="arm_elbow_flex_servo_link"/>
-  </joint>
-  <link name="arm_elbow_flex_servo_link">
-    <inertial>
-      <mass value="0.055"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="0.000017012" ixy="0.0" ixz="0.0" iyy="0.000013258" iyz="0.0" izz="0.000009483"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/ax12_box.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="Black"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 -0.01241"/>
-      <geometry>
-        <box size="0.025 0.038 0.04762"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="arm_elbow_flex_servo_link">
-    <material>Gazebo/Black</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <joint name="arm_elbow_flex_joint" type="revolute">
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-    <axis xyz="0 1 0"/>
-    <limit effort="30" lower="-2.42" upper="2.38" velocity="1.571"/>
-    <dynamics friction="0.13"/>
-    <parent link="arm_elbow_flex_servo_link"/>
-    <child link="arm_elbow_flex_link"/>
-  </joint>
-  <link name="arm_elbow_flex_link">
-    <inertial>
-      <mass value="0.016"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="6.67866666667e-06" ixy="0" ixz="0" iyy="4.18166666667e-06" iyz="0" izz="8.76966666667e-06"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/F4.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="Gray"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.0215"/>
-      <geometry>
-        <box size="0.028 0.0485 0.065"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="arm_elbow_flex_link">
-    <material>Gazebo/Gray</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <joint name="arm_elbow_F10_0_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0.0545"/>
-    <parent link="arm_elbow_flex_link"/>
-    <child link="arm_elbow_F10_0_link"/>
-  </joint>
-  <link name="arm_elbow_F10_0_link">
-    <inertial>
-      <mass value="0.004"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="2.13666666667e-07" ixy="0" ixz="0" iyy="6.89666666667e-07" iyz="0" izz="4.86666666667e-07"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/F10.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="Gray"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-      <geometry>
-        <box size="0.025 0.038 0.004"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="arm_elbow_F10_0_link">
-    <material>Gazebo/Gray</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <joint name="arm_elbow_F10_1_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0.004"/>
-    <parent link="arm_elbow_F10_0_link"/>
-    <child link="arm_elbow_F10_1_link"/>
-  </joint>
-  <link name="arm_elbow_F10_1_link">
-    <inertial>
-      <mass value="0.004"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="2.13666666667e-07" ixy="0" ixz="0" iyy="6.89666666667e-07" iyz="0" izz="4.86666666667e-07"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/F10.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="Gray"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-      <geometry>
-        <box size="0.025 0.038 0.004"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="arm_elbow_F10_1_link">
-    <material>Gazebo/Gray</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <joint name="arm_elbow_F10_2_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0.004"/>
-    <parent link="arm_elbow_F10_1_link"/>
-    <child link="arm_elbow_F10_2_link"/>
-  </joint>
-  <link name="arm_elbow_F10_2_link">
-    <inertial>
-      <mass value="0.004"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="2.13666666667e-07" ixy="0" ixz="0" iyy="6.89666666667e-07" iyz="0" izz="4.86666666667e-07"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/F10.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="Gray"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-      <geometry>
-        <box size="0.025 0.038 0.004"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="arm_elbow_F10_2_link">
-    <material>Gazebo/Gray</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <joint name="arm_elbow_F3_0_joint" type="fixed">
-    <origin rpy="0 3.14159 0" xyz="0 0 0.002"/>
-    <parent link="arm_elbow_F10_2_link"/>
-    <child link="arm_elbow_F3_0_link"/>
-  </joint>
-  <link name="arm_elbow_F3_0_link">
-    <inertial>
-      <mass value="0.005"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="2.94166666667e-07" ixy="0" ixz="0" iyy="8.62083333333e-07" iyz="0" izz="6.35416666667e-07"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/F3.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="Gray"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 -0.0045"/>
-      <geometry>
-        <box size="0.025 0.038 0.009"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="arm_elbow_F3_0_link">
-    <material>Gazebo/Gray</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <joint name="arm_wrist_flex_servo_joint" type="fixed">
-    <origin rpy="0 3.14159 0" xyz="0 0 -0.0415"/>
-    <parent link="arm_elbow_F3_0_link"/>
-    <child link="arm_wrist_flex_servo_link"/>
-  </joint>
-  <link name="arm_wrist_flex_servo_link">
-    <inertial>
-      <mass value="0.055"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="0.000017012" ixy="0.0" ixz="0.0" iyy="0.000013258" iyz="0.0" izz="0.000009483"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/ax12_box.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="Black"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 -0.01241"/>
-      <geometry>
-        <box size="0.025 0.038 0.04762"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="arm_wrist_flex_servo_link">
-    <material>Gazebo/Black</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <joint name="arm_wrist_flex_joint" type="revolute">
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-    <axis xyz="0 1 0"/>
-    <limit effort="30" lower="-1.72" upper="1.68" velocity="1.571"/>
-    <dynamics friction="0.13"/>
-    <parent link="arm_wrist_flex_servo_link"/>
-    <child link="arm_wrist_flex_link"/>
-  </joint>
-  <link name="arm_wrist_flex_link">
-    <inertial>
-      <mass value="0.01"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="1.69270833333e-06" ixy="0" ixz="0" iyy="2.48104166667e-06" iyz="0" izz="3.13208333333e-06"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/F2.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="Gray"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.00775"/>
-      <geometry>
-        <box size="0.025 0.0485 0.0375"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="arm_wrist_flex_link">
-    <material>Gazebo/Gray</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <joint name="arm_wrist_F3_0_joint" type="fixed">
-    <origin rpy="0 3.14159 -3.14159" xyz="0 0 0.0265"/>
-    <parent link="arm_wrist_flex_link"/>
-    <child link="arm_wrist_F3_0_link"/>
-  </joint>
-  <link name="arm_wrist_F3_0_link">
-    <inertial>
-      <mass value="0.005"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="2.94166666667e-07" ixy="0" ixz="0" iyy="8.62083333333e-07" iyz="0" izz="6.35416666667e-07"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/F3.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="Gray"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 -0.0045"/>
-      <geometry>
-        <box size="0.025 0.038 0.009"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="arm_wrist_F3_0_link">
-    <material>Gazebo/Gray</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <joint name="gripper_servo_joint" type="fixed">
-    <origin rpy="1.570795 3.14159 1.570795" xyz="0 0 -0.019"/>
-    <parent link="arm_wrist_F3_0_link"/>
-    <child link="gripper_servo_link"/>
-  </joint>
-  <link name="gripper_servo_link">
-    <inertial>
-      <mass value="0.055"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="0.000017012" ixy="0.0" ixz="0.0" iyy="0.000013258" iyz="0.0" izz="0.000009483"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/ax12_box.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="Black"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 -0.01241"/>
-      <geometry>
-        <box size="0.025 0.038 0.04762"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="gripper_servo_link">
-    <material>Gazebo/Black</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <joint name="gripper_finger_base_joint" type="fixed">
-    <origin rpy="1.570795 -3.14159 1.570795" xyz="0.01  0.038 0"/>
-    <parent link="gripper_servo_link"/>
-    <child link="gripper_finger_base_link"/>
-  </joint>
-  <link name="gripper_finger_base_link">
-    <inertial>
-      <mass value="1e-05"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="4.69083333333e-09" ixy="0" ixz="0" iyy="1.33666666667e-09" iyz="0" izz="6.02083333333e-09"/>
-    </inertial>
-    <visual>
-      <origin rpy="3.14159 -1.570795 1.570795" xyz="0.016 0 -.015 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/pincher_finger_base.stl" scale=".01 .01 .01"/>
-      </geometry>
-      <material name="Gray"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.015 0 -0.0135"/>
-      <geometry>
-        <box size="0.002 0.040 0.075"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="gripper_finger_base_link">
-    <material>Gazebo/Gray</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <!-- Finger 1 -->
-  <joint name="gripper_joint" type="prismatic">
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-    <axis xyz="1 0 0"/>
-    <limit effort="30" lower="0.002" upper="0.031" velocity="0.5"/>
-    <parent link="gripper_servo_link"/>
-    <child link="gripper_active_link"/>
-  </joint>
-  <link name="gripper_active_link">
-    <inertial>
-      <mass value="1e-05"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="1.09333333333e-09" ixy="0" ixz="0" iyy="1.73333333333e-09" iyz="0" izz="6.66666666667e-10"/>
-    </inertial>
-    <visual>
-      <origin rpy="-1.570795 0 0" xyz="-0.02 0.023 0"/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/pincher_finger.stl" scale=".01 .01 .01"/>
-      </geometry>
-      <material name="Gray"/>
-    </visual>
-    <collision>
-      <origin rpy="-1.570795 0 0" xyz="-0.02 0.023 0"/>
-      <geometry>
-        <box size="0.036 0.028 0.004"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="gripper_active_link">
-    <material>Gazebo/Gray</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <!-- Finger 2 -->
-  <!-- Note: currently static but should be a Mimic of Finger 1 -->
-  <joint name="gripper2_joint" type="fixed">
-    <parent link="gripper_servo_link"/>
-    <child link="gripper_active2_link"/>
-  </joint>
-  <link name="gripper_active2_link">
-    <inertial>
-      <mass value="1e-05"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="1.09333333333e-09" ixy="0" ixz="0" iyy="1.73333333333e-09" iyz="0" izz="6.66666666667e-10"/>
-    </inertial>
-    <visual>
-      <origin rpy="-1.570795 3.14159 0" xyz="-0.02 0.023 0"/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/pincher_finger.stl" scale=".01 .01 .01"/>
-      </geometry>
-      <material name="Gray"/>
-    </visual>
-    <collision>
-      <origin rpy="-1.570795 3.14159 0" xyz="-0.02 0.023 0"/>
-      <geometry>
-        <box size="0.036 0.028 0.004"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="gripper_active2_link">
-    <material>Gazebo/Gray</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <!-- Using Mimic -->
-  <!-- gripper 2 joint -->
-  <!--  ### Mimic would be better but causes Missing Joint error in Moveit 
-      <joint name="gripper2_joint" type="prismatic">
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <axis xyz="0 0 1"/>
-      <limit effort="30" velocity="0.5" lower="0.002" upper="0.031"/>
-      <parent link="gripper_servo_link"/>
-      <child link="gripper_active2_link"/>
-      <mimic joint="gripper_joint" multiplier="-.5"  offset = "0"/>
-    </joint-->
-</robot>

src/turtlebot_arm/turtlebot_arm_description/urdf/pincher_arm.urdf.xacro

@@ -1,31 +0,0 @@
-<?xml version="1.0"?>
-<!-- Describe URDF for PhantomX Pincher Arm -->
-<robot name="turtlebot_arm" xmlns:xacro="http://ros.org/wiki/xacro">
-
-    <!-- We can configure joints velocity limit and lower/upper limits
-          to allow access to different operational areas, e.g. left handed vs. right handed robot -->
-    <xacro:property name="joints_vlimit" value="1.571"/>
-    <xacro:property name="pan_llimit" value="-2.617"/>
-    <xacro:property name="pan_ulimit" value="2.617"/>
-    <xacro:property name="shoulder_llimit" value="-2.2"/>
-    <xacro:property name="shoulder_ulimit" value="2.16"/>
-    <xacro:property name="elbow_llimit" value="-2.42"/>
-    <xacro:property name="elbow_ulimit" value="2.38"/>
-    <xacro:property name="wrist_llimit" value="-1.72"/>
-    <xacro:property name="wrist_ulimit" value="1.68"/>
-
-    <!-- Included URDF Files -->
-    
-     <!-- Pincher arm is same as Turtlebot -->
-    <xacro:include filename="$(find turtlebot_arm_description)/urdf/turtlebot_arm.xacro"/>
-    
-    <!-- As we don't have here a turtlebot base, add a base_link link as its location reference -->
-    <link name="base_link"/>
-
-    <!-- Turtlebot arm macro -->
-    <xacro:turtlebot_arm parent="base_link" color="Gray" gripper_color="Gray"   pincher_gripper="true" turtlebot_gripper="false">
-      <!-- Place the "floating" arm at the location it should be if mounted on a turtlebot,
-           as pick and place and other demos assume this location -->
-      <origin xyz="0.1 0.03 0.435"/>
-    </xacro:turtlebot_arm>
-</robot>

src/turtlebot_arm/turtlebot_arm_description/urdf/pincher_gripper.xacro

@@ -1,52 +0,0 @@
-<?xml version="1.0"?>
-<!-- PhantomX Pincher Arm gripper URDF-->
-<robot xmlns:xacro="http://ros.org/wiki/xacro">
-    <xacro:bioloid_F3_fixed parent="arm_wrist_flex_link" name="arm_wrist_F3_0" color="${color}">
-      <origin xyz="0 0 ${F2_HEIGHT}" rpy="0 ${M_PI} ${-M_PI}"/>
-    </xacro:bioloid_F3_fixed>
-    
-    <!-- gripper joint -->
-    <xacro:dynamixel_AX12_fixed parent="arm_wrist_F3_0_link" name="gripper_servo" >
-      <origin xyz="0 0 ${-AX12_WIDTH/2}" rpy="${M_PI/2} ${M_PI} ${M_PI/2}"/>
-    </xacro:dynamixel_AX12_fixed>
-    
-    <xacro:gripper_finger_base parent="gripper_servo_link" name="gripper_finger_base" color="${color}">
-      <origin xyz="0.01  ${AX12_WIDTH} 0" rpy="${M_PI/2} ${-M_PI} ${M_PI/2}"/>
-    </xacro:gripper_finger_base>
-  
-     <!-- Finger 1 --> 
-    <joint name="gripper_joint" type="prismatic">
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <axis xyz="1 0 0"/>
-      <limit effort="30" velocity="0.5" lower="0.002" upper="0.031"/>
-      <parent link="gripper_servo_link"/>
-      <child link="gripper_active_link"/>
-    </joint> 
-    
-    <xacro:pincher_gripper name="gripper_active" color="${color}" >
-      <origin xyz="-0.0086 0.0414 0" rpy="${-M_PI/2} 0 0"/>
-    </xacro:pincher_gripper>
-     
-    <!-- Finger 2 --> 
-    <!-- Note: currently static but should be a Mimic of Finger 1 -->
-    <joint name="gripper2_joint" type="fixed">
-      <parent link="gripper_servo_link"/>
-      <child link="gripper_active2_link"/>
-    </joint>  
-    
-    <xacro:pincher_gripper name="gripper_active2" color="${color}" >
-      <origin xyz="-0.0314 0.0414 0" rpy="${-M_PI/2} ${M_PI} 0"/>
-    </xacro:pincher_gripper>
-      
-    <!-- Using Mimic -->
-    <!-- gripper 2 joint -->
-    <!--  ### Mimic would be better but causes Missing Joint error in Moveit 
-      <joint name="gripper2_joint" type="prismatic">
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <axis xyz="0 0 1"/>
-      <limit effort="30" velocity="0.5" lower="0.002" upper="0.031"/>
-      <parent link="gripper_servo_link"/>
-      <child link="gripper_active2_link"/>
-      <mimic joint="gripper_joint" multiplier="-.5"  offset = "0"/>
-    </joint-->  
-</robot>

src/turtlebot_arm/turtlebot_arm_description/urdf/turtlebot_arm.urdf

@@ -1,709 +0,0 @@
-<?xml version="1.0" ?>
-<!-- =================================================================================== -->
-<!-- |    This document was autogenerated by xacro from turtlebot_arm.urdf.xacro       | -->
-<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
-<!-- =================================================================================== -->
-<!-- Describe URDF for Turtlebot Arm -->
-<robot name="turtlebot_arm" xmlns:xacro="http://ros.org/wiki/xacro">
-  <material name="White">
-    <color rgba="0.87 0.90 0.87 1.0"/>
-  </material>
-  <material name="Black">
-    <color rgba="0.08 0.08 0.08 1.0"/>
-  </material>
-  <material name="Gray">
-    <color rgba="0.2 0.2 0.21 1.0"/>
-  </material>
-  <material name="Green">
-    <color rgba="0.22 0.32 0.14 1.0"/>
-  </material>
-  <!-- As we don't have here a turtlebot base, add a base_link link as its location reference -->
-  <link name="base_link"/>
-  <link name="arm_base_link"/>
-  <joint name="arm_base_joint" type="fixed">
-    <origin xyz="0.1 0.03 0.435"/>
-    <parent link="base_link"/>
-    <child link="arm_base_link"/>
-  </joint>
-  <!-- fake gripper_link joint gives us a free servo!
-         this makes us 5DOF and saves you $44.90
-         that's a lot of coin! -->
-  <link name="gripper_link"/>
-  <joint name="gripper_link_joint" type="revolute">
-    <origin rpy="0 -1.57 0" xyz="0 0 0.112"/>
-    <parent link="arm_wrist_flex_link"/>
-    <child link="gripper_link"/>
-    <limit effort="30" lower="-3.14" upper="3.14" velocity="1"/>
-    <axis xyz="1 0 0"/>
-  </joint>
-  <joint name="arm_shoulder_pan_servo_joint" type="fixed">
-    <origin rpy="1.570795 0 1.570795" xyz="0 0 0"/>
-    <parent link="arm_base_link"/>
-    <child link="arm_shoulder_pan_servo_link"/>
-  </joint>
-  <link name="arm_shoulder_pan_servo_link">
-    <inertial>
-      <mass value="0.055"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="0.000017012" ixy="0.0" ixz="0.0" iyy="0.000013258" iyz="0.0" izz="0.000009483"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/ax12_box.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="Black"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 -0.01241"/>
-      <geometry>
-        <box size="0.025 0.038 0.04762"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="arm_shoulder_pan_servo_link">
-    <material>Gazebo/Black</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <joint name="arm_shoulder_pan_joint" type="revolute">
-    <origin rpy="-1.570795 1.570795 3.14159" xyz="0 0.019 0"/>
-    <axis xyz="0 0 -1"/>
-    <limit effort="30" lower="-2.617" upper="2.617" velocity="1.571"/>
-    <dynamics friction="0.13"/>
-    <parent link="arm_shoulder_pan_servo_link"/>
-    <child link="arm_shoulder_pan_link"/>
-  </joint>
-  <link name="arm_shoulder_pan_link">
-    <inertial>
-      <mass value="0.008"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="4.70666666667e-07" ixy="0" ixz="0" iyy="1.37933333333e-06" iyz="0" izz="1.01666666667e-06"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/F3.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="White"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 -0.0045"/>
-      <geometry>
-        <box size="0.025 0.038 0.009"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="arm_shoulder_pan_link">
-    <material>Gazebo/White</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <joint name="arm_shoulder_lift_servo_joint" type="fixed">
-    <origin rpy="0 3.14159 0" xyz="0 0 -0.0415"/>
-    <parent link="arm_shoulder_pan_link"/>
-    <child link="arm_shoulder_lift_servo_link"/>
-  </joint>
-  <link name="arm_shoulder_lift_servo_link">
-    <inertial>
-      <mass value="0.055"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="0.000017012" ixy="0.0" ixz="0.0" iyy="0.000013258" iyz="0.0" izz="0.000009483"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/ax12_box.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="Black"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 -0.01241"/>
-      <geometry>
-        <box size="0.025 0.038 0.04762"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="arm_shoulder_lift_servo_link">
-    <material>Gazebo/Black</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <joint name="arm_shoulder_lift_joint" type="revolute">
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-    <axis xyz="0 1 0"/>
-    <limit effort="30" lower="-2.617" upper="2.617" velocity="1.571"/>
-    <dynamics friction="0.13"/>
-    <parent link="arm_shoulder_lift_servo_link"/>
-    <child link="arm_shoulder_lift_link"/>
-  </joint>
-  <link name="arm_shoulder_lift_link">
-    <inertial>
-      <mass value="0.016"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="6.67866666667e-06" ixy="0" ixz="0" iyy="4.18166666667e-06" iyz="0" izz="8.76966666667e-06"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/F4.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="White"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.0215"/>
-      <geometry>
-        <box size="0.028 0.0485 0.065"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="arm_shoulder_lift_link">
-    <material>Gazebo/White</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <joint name="arm_shoulder_F10_0_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0.0545"/>
-    <parent link="arm_shoulder_lift_link"/>
-    <child link="arm_shoulder_F10_0_link"/>
-  </joint>
-  <link name="arm_shoulder_F10_0_link">
-    <inertial>
-      <mass value="0.004"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="2.13666666667e-07" ixy="0" ixz="0" iyy="6.89666666667e-07" iyz="0" izz="4.86666666667e-07"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/F10.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="White"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-      <geometry>
-        <box size="0.025 0.038 0.004"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="arm_shoulder_F10_0_link">
-    <material>Gazebo/White</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <joint name="arm_shoulder_F10_1_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0.004"/>
-    <parent link="arm_shoulder_F10_0_link"/>
-    <child link="arm_shoulder_F10_1_link"/>
-  </joint>
-  <link name="arm_shoulder_F10_1_link">
-    <inertial>
-      <mass value="0.004"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="2.13666666667e-07" ixy="0" ixz="0" iyy="6.89666666667e-07" iyz="0" izz="4.86666666667e-07"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/F10.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="White"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-      <geometry>
-        <box size="0.025 0.038 0.004"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="arm_shoulder_F10_1_link">
-    <material>Gazebo/White</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <joint name="arm_shoulder_F10_2_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0.004"/>
-    <parent link="arm_shoulder_F10_1_link"/>
-    <child link="arm_shoulder_F10_2_link"/>
-  </joint>
-  <link name="arm_shoulder_F10_2_link">
-    <inertial>
-      <mass value="0.004"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="2.13666666667e-07" ixy="0" ixz="0" iyy="6.89666666667e-07" iyz="0" izz="4.86666666667e-07"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/F10.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="White"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-      <geometry>
-        <box size="0.025 0.038 0.004"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="arm_shoulder_F10_2_link">
-    <material>Gazebo/White</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <joint name="arm_shoulder_F3_0_joint" type="fixed">
-    <origin rpy="0 3.14159 0" xyz="0 0 0.002"/>
-    <parent link="arm_shoulder_F10_2_link"/>
-    <child link="arm_shoulder_F3_0_link"/>
-  </joint>
-  <link name="arm_shoulder_F3_0_link">
-    <inertial>
-      <mass value="0.005"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="2.94166666667e-07" ixy="0" ixz="0" iyy="8.62083333333e-07" iyz="0" izz="6.35416666667e-07"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/F3.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="White"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 -0.0045"/>
-      <geometry>
-        <box size="0.025 0.038 0.009"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="arm_shoulder_F3_0_link">
-    <material>Gazebo/White</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <joint name="arm_elbow_flex_servo_joint" type="fixed">
-    <origin rpy="0 3.14159 0" xyz="0 0 -0.0415"/>
-    <parent link="arm_shoulder_F3_0_link"/>
-    <child link="arm_elbow_flex_servo_link"/>
-  </joint>
-  <link name="arm_elbow_flex_servo_link">
-    <inertial>
-      <mass value="0.055"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="0.000017012" ixy="0.0" ixz="0.0" iyy="0.000013258" iyz="0.0" izz="0.000009483"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/ax12_box.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="Black"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 -0.01241"/>
-      <geometry>
-        <box size="0.025 0.038 0.04762"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="arm_elbow_flex_servo_link">
-    <material>Gazebo/Black</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <joint name="arm_elbow_flex_joint" type="revolute">
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-    <axis xyz="0 1 0"/>
-    <limit effort="30" lower="-2.617" upper="2.617" velocity="1.571"/>
-    <dynamics friction="0.13"/>
-    <parent link="arm_elbow_flex_servo_link"/>
-    <child link="arm_elbow_flex_link"/>
-  </joint>
-  <link name="arm_elbow_flex_link">
-    <inertial>
-      <mass value="0.016"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="6.67866666667e-06" ixy="0" ixz="0" iyy="4.18166666667e-06" iyz="0" izz="8.76966666667e-06"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/F4.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="White"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.0215"/>
-      <geometry>
-        <box size="0.028 0.0485 0.065"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="arm_elbow_flex_link">
-    <material>Gazebo/White</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <joint name="arm_elbow_F10_0_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0.0545"/>
-    <parent link="arm_elbow_flex_link"/>
-    <child link="arm_elbow_F10_0_link"/>
-  </joint>
-  <link name="arm_elbow_F10_0_link">
-    <inertial>
-      <mass value="0.004"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="2.13666666667e-07" ixy="0" ixz="0" iyy="6.89666666667e-07" iyz="0" izz="4.86666666667e-07"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/F10.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="White"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-      <geometry>
-        <box size="0.025 0.038 0.004"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="arm_elbow_F10_0_link">
-    <material>Gazebo/White</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <joint name="arm_elbow_F10_1_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0.004"/>
-    <parent link="arm_elbow_F10_0_link"/>
-    <child link="arm_elbow_F10_1_link"/>
-  </joint>
-  <link name="arm_elbow_F10_1_link">
-    <inertial>
-      <mass value="0.004"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="2.13666666667e-07" ixy="0" ixz="0" iyy="6.89666666667e-07" iyz="0" izz="4.86666666667e-07"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/F10.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="White"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-      <geometry>
-        <box size="0.025 0.038 0.004"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="arm_elbow_F10_1_link">
-    <material>Gazebo/White</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <joint name="arm_elbow_F10_2_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0.004"/>
-    <parent link="arm_elbow_F10_1_link"/>
-    <child link="arm_elbow_F10_2_link"/>
-  </joint>
-  <link name="arm_elbow_F10_2_link">
-    <inertial>
-      <mass value="0.004"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="2.13666666667e-07" ixy="0" ixz="0" iyy="6.89666666667e-07" iyz="0" izz="4.86666666667e-07"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/F10.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="White"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-      <geometry>
-        <box size="0.025 0.038 0.004"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="arm_elbow_F10_2_link">
-    <material>Gazebo/White</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <joint name="arm_elbow_F3_0_joint" type="fixed">
-    <origin rpy="0 3.14159 0" xyz="0 0 0.002"/>
-    <parent link="arm_elbow_F10_2_link"/>
-    <child link="arm_elbow_F3_0_link"/>
-  </joint>
-  <link name="arm_elbow_F3_0_link">
-    <inertial>
-      <mass value="0.005"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="2.94166666667e-07" ixy="0" ixz="0" iyy="8.62083333333e-07" iyz="0" izz="6.35416666667e-07"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/F3.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="White"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 -0.0045"/>
-      <geometry>
-        <box size="0.025 0.038 0.009"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="arm_elbow_F3_0_link">
-    <material>Gazebo/White</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <joint name="arm_wrist_flex_servo_joint" type="fixed">
-    <origin rpy="0 3.14159 0" xyz="0 0 -0.0415"/>
-    <parent link="arm_elbow_F3_0_link"/>
-    <child link="arm_wrist_flex_servo_link"/>
-  </joint>
-  <link name="arm_wrist_flex_servo_link">
-    <inertial>
-      <mass value="0.055"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="0.000017012" ixy="0.0" ixz="0.0" iyy="0.000013258" iyz="0.0" izz="0.000009483"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/ax12_box.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="Black"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 -0.01241"/>
-      <geometry>
-        <box size="0.025 0.038 0.04762"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="arm_wrist_flex_servo_link">
-    <material>Gazebo/Black</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <joint name="arm_wrist_flex_joint" type="revolute">
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-    <axis xyz="0 1 0"/>
-    <limit effort="30" lower="-1.745" upper="1.745" velocity="1.571"/>
-    <dynamics friction="0.13"/>
-    <parent link="arm_wrist_flex_servo_link"/>
-    <child link="arm_wrist_flex_link"/>
-  </joint>
-  <link name="arm_wrist_flex_link">
-    <inertial>
-      <mass value="0.01"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="1.69270833333e-06" ixy="0" ixz="0" iyy="2.48104166667e-06" iyz="0" izz="3.13208333333e-06"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/F2.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="White"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.00775"/>
-      <geometry>
-        <box size="0.025 0.0485 0.0375"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="arm_wrist_flex_link">
-    <material>Gazebo/White</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <joint name="arm_wrist_F3_0_joint" type="fixed">
-    <origin rpy="0 3.14159 -1.570795" xyz="0 0.016 0.0265"/>
-    <parent link="arm_wrist_flex_link"/>
-    <child link="arm_wrist_F3_0_link"/>
-  </joint>
-  <link name="arm_wrist_F3_0_link">
-    <inertial>
-      <mass value="0.005"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="2.94166666667e-07" ixy="0" ixz="0" iyy="8.62083333333e-07" iyz="0" izz="6.35416666667e-07"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/F3.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="White"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 -0.0045"/>
-      <geometry>
-        <box size="0.025 0.038 0.009"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="arm_wrist_F3_0_link">
-    <material>Gazebo/White</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <joint name="gripper_servo_joint" type="fixed">
-    <origin rpy="3.14159 1.570795 0" xyz="-0.02275 0 -0.019"/>
-    <parent link="arm_wrist_F3_0_link"/>
-    <child link="gripper_servo_link"/>
-  </joint>
-  <link name="gripper_servo_link">
-    <inertial>
-      <mass value="0.055"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="0.000017012" ixy="0.0" ixz="0.0" iyy="0.000013258" iyz="0.0" izz="0.000009483"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/ax12_box.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="Black"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 -0.01241"/>
-      <geometry>
-        <box size="0.025 0.038 0.04762"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="gripper_servo_link">
-    <material>Gazebo/Black</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <!-- finger 1 -->
-  <joint name="gripper_joint" type="revolute">
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-    <axis xyz="0 1 0"/>
-    <limit effort="30" lower="-0.40" upper="0.46" velocity="0.785"/>
-    <parent link="gripper_servo_link"/>
-    <child link="gripper_active_link"/>
-  </joint>
-  <link name="gripper_active_link">
-    <inertial>
-      <mass value="0.010"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="0.000001" ixy="0.0" ixz="0.0" iyy="0.000001" iyz="0.0" izz="0.000001"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/F2.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="White"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.00775"/>
-      <geometry>
-        <box size="0.025 0.0485 0.0375"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="gripper_active_finger_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0.0265"/>
-    <parent link="gripper_active_link"/>
-    <child link="gripper_active_finger_link"/>
-  </joint>
-  <link name="gripper_active_finger_link">
-    <inertial>
-      <mass value="0.015"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="8.129225e-06" ixy="0" ixz="0" iyy="9.469562625e-06" iyz="0" izz="2.271562625e-06"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/turtlebot_finger.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="Green"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.02645 0.0 -0.00655"/>
-      <geometry>
-        <box size="0.0783 0.03801 0.0193"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="gripper_active_finger_link">
-    <material>Gazebo/Green</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <joint name="gripper_static_joint" type="fixed">
-    <origin rpy="0 3.14159 0" xyz="0 0 -0.0415"/>
-    <parent link="gripper_servo_link"/>
-    <child link="gripper_static_link"/>
-  </joint>
-  <link name="gripper_static_link">
-    <inertial>
-      <mass value="0.005"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="2.94166666667e-07" ixy="0" ixz="0" iyy="8.62083333333e-07" iyz="0" izz="6.35416666667e-07"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/F3.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="White"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.0 0.0 -0.0045"/>
-      <geometry>
-        <box size="0.025 0.038 0.009"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="gripper_static_link">
-    <material>Gazebo/White</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-  <joint name="gripper_static_finger_joint" type="fixed">
-    <origin rpy="0 0 3.14159" xyz="0 0 0"/>
-    <parent link="gripper_static_link"/>
-    <child link="gripper_static_finger_link"/>
-  </joint>
-  <link name="gripper_static_finger_link">
-    <inertial>
-      <mass value="0.015"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="8.129225e-06" ixy="0" ixz="0" iyy="9.469562625e-06" iyz="0" izz="2.271562625e-06"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz=" 0 0 0 "/>
-      <geometry>
-        <mesh filename="package://turtlebot_arm_description/meshes/turtlebot_finger.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="Green"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0.02645 0.0 -0.00655"/>
-      <geometry>
-        <box size="0.0783 0.03801 0.0193"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="gripper_static_finger_link">
-    <material>Gazebo/Green</material>
-    <selfCollide>true</selfCollide>
-    <gravity>true</gravity>
-  </gazebo>
-</robot>

src/turtlebot_arm/turtlebot_arm_description/urdf/turtlebot_arm.urdf.xacro

@@ -1,30 +0,0 @@
-<?xml version="1.0"?>
-<!-- Describe URDF for Turtlebot Arm -->
-<robot name="turtlebot_arm" xmlns:xacro="http://ros.org/wiki/xacro">
-
-    <!-- We can configure joints velocity limit and lower/upper limits
-         to allow access to different operational areas, e.g. left handed vs. right handed robot -->
-    <xacro:property name="joints_vlimit" value="1.571"/>
-    <xacro:property name="pan_llimit" value="-2.617"/>
-    <xacro:property name="pan_ulimit" value="2.617"/>
-    <xacro:property name="shoulder_llimit" value="-2.617"/>
-    <xacro:property name="shoulder_ulimit" value="2.617"/>
-    <xacro:property name="elbow_llimit" value="-2.617"/>
-    <xacro:property name="elbow_ulimit" value="2.617"/>
-    <xacro:property name="wrist_llimit" value="-1.745"/>
-    <xacro:property name="wrist_ulimit" value="1.745"/>
-
-    <!-- Included URDF Files -->   
-    <xacro:include filename="$(find turtlebot_arm_description)/urdf/turtlebot_arm.xacro"/>
-
-    <!-- As we don't have here a turtlebot base, add a base_link link as its location reference -->
-    <link name="base_link"/>
-
-    <!-- Turtlebot arm macro -->
-    <xacro:turtlebot_arm parent="base_link" color="White" gripper_color="Green"
-                         pincher_gripper="false" turtlebot_gripper="true">
-      <!-- Place the "floating" arm at the location it should be if mounted on a turtlebot,
-           as pick and place and other demos assume this location -->
-      <origin xyz="0.1 0.03 0.435"/>
-    </xacro:turtlebot_arm>
-</robot>

src/turtlebot_arm/turtlebot_arm_description/urdf/turtlebot_arm.xacro

@@ -1,109 +0,0 @@
-<?xml version="1.0"?>
-<!-- Arm description for Turtlebot Arm and PhantomX Pincher arm -->
-<robot xmlns:xacro="http://ros.org/wiki/xacro">
-  <xacro:include filename="$(find turtlebot_arm_description)/urdf/arm_hardware.xacro"/>
-
-  <material name="White">
-    <color rgba="0.87 0.90 0.87 1.0"/>
-  </material>
-  <material name="Black">
-    <color rgba="0.08 0.08 0.08 1.0"/>
-  </material>
-  <material name="Gray">
-    <color rgba="0.2 0.2 0.21 1.0"/>
-  </material>
-  <material name="Green">
-    <color rgba="0.22 0.32 0.14 1.0"/>
-  </material>
-
-  <xacro:macro name="turtlebot_arm" params="parent color pincher_gripper turtlebot_gripper gripper_color *origin">
-    <link name="arm_base_link"/>
-    <joint name="arm_base_joint" type="fixed">
-      <xacro:insert_block name="origin"/>
-      <parent link="${parent}"/>
-      <child link="arm_base_link"/>
-    </joint>
-
-    <!-- fake gripper_link joint gives us a free servo!
-         this makes us 5DOF and saves you $44.90
-         that's a lot of coin! -->
-    <link name="gripper_link"/>
-    <joint name="gripper_link_joint" type="revolute">
-      <origin xyz="0 0 0.112" rpy="0 -1.57 0"/>
-      <parent link="arm_wrist_flex_link"/>
-      <child link="gripper_link"/>
-      <limit effort="30" velocity="1" lower="-3.14" upper="3.14"/>
-      <axis xyz="1 0 0"/>
-    </joint>
-
-    <!-- shoulder pan joint -->
-    <xacro:dynamixel_AX12_fixed parent="arm_base_link" name="arm_shoulder_pan_servo">
-      <origin xyz="0 0 0" rpy="${M_PI/2} 0 ${M_PI/2}"/>
-    </xacro:dynamixel_AX12_fixed>
-    <xacro:bioloid_F3_revolute parent="arm_shoulder_pan_servo_link" name="arm_shoulder_pan" color="${color}"
-                               vlimit="${joints_vlimit}" llimit="${pan_llimit}" ulimit="${pan_ulimit}">
-      <origin xyz="0 ${AX12_WIDTH/2} 0" rpy="${-M_PI/2} ${M_PI/2} ${M_PI}"/>
-    </xacro:bioloid_F3_revolute>
-
-    <!-- shoulder lift joint -->
-    <xacro:dynamixel_AX12_fixed parent="arm_shoulder_pan_link" name="arm_shoulder_lift_servo">
-      <origin xyz="0 0 ${-AX12_HEIGHT-F10_HEIGHT+0.001}" rpy="0 ${M_PI} 0"/>
-    </xacro:dynamixel_AX12_fixed>
-    <xacro:bioloid_F4_revolute parent="arm_shoulder_lift_servo_link" name="arm_shoulder_lift" color="${color}"
-                               vlimit="${joints_vlimit}" llimit="${shoulder_llimit}" ulimit="${shoulder_ulimit}">
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-    </xacro:bioloid_F4_revolute>
-    <xacro:bioloid_F10_fixed parent="arm_shoulder_lift_link" name="arm_shoulder_F10_0" color="${color}">
-      <origin xyz="0 0 ${F4_HEIGHT+F10_HEIGHT/2}" rpy="0 0 0"/>
-    </xacro:bioloid_F10_fixed>
-    <xacro:bioloid_F10_fixed parent="arm_shoulder_F10_0_link" name="arm_shoulder_F10_1" color="${color}">
-      <origin xyz="0 0 ${F10_HEIGHT}" rpy="0 0 0"/>
-    </xacro:bioloid_F10_fixed>
-    <xacro:bioloid_F10_fixed parent="arm_shoulder_F10_1_link" name="arm_shoulder_F10_2" color="${color}">
-      <origin xyz="0 0 ${F10_HEIGHT}" rpy="0 0 0"/>
-    </xacro:bioloid_F10_fixed>
-    <xacro:bioloid_F3_fixed parent="arm_shoulder_F10_2_link" name="arm_shoulder_F3_0" color="${color}">
-      <origin xyz="0 0 ${F10_HEIGHT/2}" rpy="0 ${M_PI} 0"/>
-    </xacro:bioloid_F3_fixed>
-
-    <!-- elbow joint -->
-    <xacro:dynamixel_AX12_fixed parent="arm_shoulder_F3_0_link" name="arm_elbow_flex_servo">
-      <origin xyz="0 0 ${-AX12_HEIGHT-F10_HEIGHT+0.001}" rpy="0 ${M_PI} 0"/>
-    </xacro:dynamixel_AX12_fixed>
-    <xacro:bioloid_F4_revolute parent="arm_elbow_flex_servo_link" name="arm_elbow_flex" color="${color}"
-                               vlimit="${joints_vlimit}" llimit="${elbow_llimit}" ulimit="${elbow_ulimit}">
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-    </xacro:bioloid_F4_revolute>
-    <xacro:bioloid_F10_fixed parent="arm_elbow_flex_link" name="arm_elbow_F10_0" color="${color}">
-      <origin xyz="0 0 ${F4_HEIGHT+F10_HEIGHT/2}" rpy="0 0 0"/>
-    </xacro:bioloid_F10_fixed>
-    <xacro:bioloid_F10_fixed parent="arm_elbow_F10_0_link" name="arm_elbow_F10_1" color="${color}">
-      <origin xyz="0 0 ${F10_HEIGHT}" rpy="0 0 0"/>
-    </xacro:bioloid_F10_fixed>
-    <xacro:bioloid_F10_fixed parent="arm_elbow_F10_1_link" name="arm_elbow_F10_2" color="${color}">
-      <origin xyz="0 0 ${F10_HEIGHT}" rpy="0 0 0"/>
-    </xacro:bioloid_F10_fixed>
-    <xacro:bioloid_F3_fixed parent="arm_elbow_F10_2_link" name="arm_elbow_F3_0" color="${color}">
-      <origin xyz="0 0 ${F10_HEIGHT/2}" rpy="0 ${M_PI} 0"/>
-    </xacro:bioloid_F3_fixed>
-
-    <!-- wrist joint -->
-    <xacro:dynamixel_AX12_fixed parent="arm_elbow_F3_0_link" name="arm_wrist_flex_servo">
-      <origin xyz="0 0 ${-AX12_HEIGHT-F10_HEIGHT+0.001}" rpy="0 ${M_PI} 0"/>
-    </xacro:dynamixel_AX12_fixed>
-    <xacro:bioloid_F2_revolute parent="arm_wrist_flex_servo_link" name="arm_wrist_flex" color="${color}"
-                               vlimit="${joints_vlimit}" llimit="${wrist_llimit}" ulimit="${wrist_ulimit}">
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-    </xacro:bioloid_F2_revolute>
-
-  <!-- gripper - Load either Turtlebot gripper or Pincher Gripper -->
-  <xacro:if value="${pincher_gripper}">
-  <xacro:include filename="$(find turtlebot_arm_description)/urdf/pincher_gripper.xacro"/>
-  </xacro:if>
-  <xacro:if value="${turtlebot_gripper}">
-  <xacro:include filename="$(find turtlebot_arm_description)/urdf/turtlebot_gripper.xacro"/>
-  </xacro:if>
-
-  </xacro:macro>
-
-</robot>

src/turtlebot_arm/turtlebot_arm_description/urdf/turtlebot_gripper.xacro

@@ -1,53 +0,0 @@
-<?xml version="1.0"?>
-<!-- Turtlebot Arm gripper URDF-->
-<robot xmlns:xacro="http://ros.org/wiki/xacro">
-    <xacro:bioloid_F3_fixed parent="arm_wrist_flex_link" name="arm_wrist_F3_0" color="${color}">
-      <origin xyz="0 0.016 ${F2_HEIGHT}" rpy="0 ${M_PI} ${-M_PI/2}"/>
-    </xacro:bioloid_F3_fixed>
-    <!-- gripper joint -->
-    <xacro:dynamixel_AX12_fixed parent="arm_wrist_F3_0_link" name="gripper_servo">
-      <origin xyz="-0.02275 0 ${-AX12_WIDTH/2}" rpy="${M_PI} ${M_PI/2} 0"/>
-    </xacro:dynamixel_AX12_fixed>
-
-    <!-- finger 1 -->
-    <joint name="gripper_joint" type="revolute">
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <axis xyz="0 1 0"/>
-      <limit effort="30" velocity="0.785" lower="-0.40" upper="0.46"/>
-      <parent link="gripper_servo_link"/>
-      <child link="gripper_active_link"/>
-    </joint>
-    <link name="gripper_active_link">
-      <inertial>
-        <mass value="0.010"/>
-        <origin xyz="0 0 0"/>
-        <inertia ixx="0.000001" ixy="0.0" ixz="0.0"
-                 iyy="0.000001" iyz="0.0"
-                 izz="0.000001"/>
-      </inertial>
-      <visual>
-        <origin xyz=" 0 0 0 " rpy="0 0 0"/>
-        <geometry>
-          <mesh filename="package://turtlebot_arm_description/meshes/F2.stl" scale="${M_SCALE} ${M_SCALE} ${M_SCALE}"/>
-        </geometry>
-        <material name="${color}"/>
-      </visual>
-      <collision>
-        <origin xyz="0.0 0.0 0.00775" rpy="0 0 0"/>
-        <geometry>
-          <box size="0.025 0.0485 0.0375"/>
-        </geometry>
-      </collision>
-    </link>
-    <xacro:finger_fixed parent="gripper_active_link" name="gripper_active_finger" color="${gripper_color}">
-      <origin xyz="0 0 ${F2_HEIGHT}" rpy="0 0 0"/>
-    </xacro:finger_fixed>
-
-    <!-- finger 2 -->
-    <xacro:bioloid_F3_fixed parent="gripper_servo_link" name="gripper_static" color="${color}">
-      <origin xyz="0 0 ${-AX12_HEIGHT-F10_HEIGHT+0.001}" rpy="0 ${M_PI} 0"/>
-    </xacro:bioloid_F3_fixed>
-    <xacro:finger_fixed parent="gripper_static_link" name="gripper_static_finger" color="${gripper_color}">
-      <origin xyz="0 0 0" rpy="0 0 ${M_PI}"/>
-    </xacro:finger_fixed>
-</robot>

src/turtlebot_arm/turtlebot_arm_ikfast_plugin/.cproject

@@ -1,42 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" standalone="no"?>
-<?fileVersion 4.0.0?><cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
-	<storageModule moduleId="org.eclipse.cdt.core.settings">
-		<cconfiguration id="cdt.managedbuild.toolchain.gnu.base.2022292065">
-			<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.base.2022292065" moduleId="org.eclipse.cdt.core.settings" name="Default">
-				<externalSettings/>
-				<extensions>
-					<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
-				</extensions>
-			</storageModule>
-			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
-				<configuration buildProperties="" id="cdt.managedbuild.toolchain.gnu.base.2022292065" name="Default" parent="org.eclipse.cdt.build.core.emptycfg">
-					<folderInfo id="cdt.managedbuild.toolchain.gnu.base.2022292065.99922374" name="/" resourcePath="">
-						<toolChain id="cdt.managedbuild.toolchain.gnu.base.2029055878" name="Linux GCC" superClass="cdt.managedbuild.toolchain.gnu.base">
-							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="cdt.managedbuild.target.gnu.platform.base.281934236" name="Debug Platform" osList="linux,hpux,aix,qnx" superClass="cdt.managedbuild.target.gnu.platform.base"/>
-							<builder id="cdt.managedbuild.target.gnu.builder.base.1408115159" managedBuildOn="false" name="Gnu Make Builder.Default" superClass="cdt.managedbuild.target.gnu.builder.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.archiver.base.1624639044" name="GCC Archiver" superClass="cdt.managedbuild.tool.gnu.archiver.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.cpp.compiler.base.1638201762" name="GCC C++ Compiler" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.c.compiler.base.152540532" name="GCC C Compiler" superClass="cdt.managedbuild.tool.gnu.c.compiler.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.c.linker.base.1741275431" name="GCC C Linker" superClass="cdt.managedbuild.tool.gnu.c.linker.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.cpp.linker.base.1987518769" name="GCC C++ Linker" superClass="cdt.managedbuild.tool.gnu.cpp.linker.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.assembler.base.79536198" name="GCC Assembler" superClass="cdt.managedbuild.tool.gnu.assembler.base"/>
-						</toolChain>
-					</folderInfo>
-				</configuration>
-			</storageModule>
-			<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
-		</cconfiguration>
-	</storageModule>
-	<storageModule moduleId="cdtBuildSystem" version="4.0.0">
-		<project id="turtlebot_arm_ikfast_plugin.null.2021836622" name="turtlebot_arm_ikfast_plugin"/>
-	</storageModule>
-	<storageModule moduleId="scannerConfiguration">
-		<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
-	</storageModule>
-	<storageModule moduleId="org.eclipse.cdt.core.LanguageSettingsProviders"/>
-</cproject>

src/turtlebot_arm/turtlebot_arm_ikfast_plugin/.project

@@ -1,27 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<projectDescription>
-	<name>turtlebot_arm_ikfast_plugin</name>
-	<comment></comment>
-	<projects>
-	</projects>
-	<buildSpec>
-		<buildCommand>
-			<name>org.eclipse.cdt.managedbuilder.core.genmakebuilder</name>
-			<triggers>clean,full,incremental,</triggers>
-			<arguments>
-			</arguments>
-		</buildCommand>
-		<buildCommand>
-			<name>org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder</name>
-			<triggers>full,incremental,</triggers>
-			<arguments>
-			</arguments>
-		</buildCommand>
-	</buildSpec>
-	<natures>
-		<nature>org.eclipse.cdt.core.cnature</nature>
-		<nature>org.eclipse.cdt.core.ccnature</nature>
-		<nature>org.eclipse.cdt.managedbuilder.core.managedBuildNature</nature>
-		<nature>org.eclipse.cdt.managedbuilder.core.ScannerConfigNature</nature>
-	</natures>
-</projectDescription>

src/turtlebot_arm/turtlebot_arm_ikfast_plugin/CHANGELOG.rst

@@ -1,17 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package turtlebot_arm_ikfast_plugin
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-0.3.3 (2014-09-20)
-------------------
-
-0.3.2 (2014-08-30)
-------------------
-
-0.3.1 (2014-08-22)
-------------------
-* Set separated URLs for website, repository and bugtracker
-
-0.3.0 (2014-08-16)
-------------------
-* First indigo release

src/turtlebot_arm/turtlebot_arm_ikfast_plugin/CMakeLists.txt

@@ -1,40 +0,0 @@
-cmake_minimum_required(VERSION 2.8.12)
-project(turtlebot_arm_ikfast_plugin)
-
-add_compile_options(-std=c++11)
-
-find_package(catkin REQUIRED COMPONENTS
-  moveit_core
-  pluginlib
-  roscpp
-  tf_conversions
-)
-
-include_directories(${catkin_INCLUDE_DIRS})
-
-catkin_package(
-  LIBRARIES
-  CATKIN_DEPENDS
-    moveit_core
-    pluginlib
-    roscpp
-    tf_conversions
-)
-
-include_directories(include)
-
-set(IKFAST_LIBRARY_NAME turtlebot_arm_arm_moveit_ikfast_plugin)
-
-find_package(LAPACK REQUIRED)
-
-add_library(${IKFAST_LIBRARY_NAME} src/turtlebot_arm_arm_ikfast_moveit_plugin.cpp)
-target_link_libraries(${IKFAST_LIBRARY_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${LAPACK_LIBRARIES})
-
-install(TARGETS ${IKFAST_LIBRARY_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
-
-install(
-  FILES
-  turtlebot_arm_moveit_ikfast_plugin_description.xml
-  DESTINATION
-  ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)

src/turtlebot_arm/turtlebot_arm_ikfast_plugin/include/ikfast.h

@@ -1,378 +0,0 @@
-// -*- coding: utf-8 -*-
-// Copyright (C) 2012 Rosen Diankov <rosen.diankov@gmail.com>
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-/** \brief  Header file for all ikfast c++ files/shared objects.
-
-    The ikfast inverse kinematics compiler is part of OpenRAVE.
-
-    The file is divided into two sections:
-    - <b>Common</b> - the abstract classes section that all ikfast share regardless of their settings
-    - <b>Library Specific</b> - the library-specific definitions, which depends on the precision/settings that the
-   library was compiled with
-
-    The defines are as follows, they are also used for the ikfast C++ class:
-
-   - IKFAST_HEADER_COMMON - common classes
-   - IKFAST_HAS_LIBRARY - if defined, will include library-specific functions. by default this is off
-   - IKFAST_CLIBRARY - Define this linking statically or dynamically to get correct visibility.
-   - IKFAST_NO_MAIN - Remove the ``main`` function, usually used with IKFAST_CLIBRARY
-   - IKFAST_ASSERT - Define in order to get a custom assert called when NaNs, divides by zero, and other invalid
-   conditions are detected.
-   - IKFAST_REAL - Use to force a custom real number type for IkReal.
-   - IKFAST_NAMESPACE - Enclose all functions and classes in this namespace, the ``main`` function is excluded.
-
- */
-#include <vector>
-#include <list>
-#include <stdexcept>
-
-#ifndef IKFAST_HEADER_COMMON
-#define IKFAST_HEADER_COMMON
-
-/// should be the same as ikfast.__version__
-#define IKFAST_VERSION 61
-
-namespace ikfast
-{
-/// \brief holds the solution for a single dof
-template <typename T>
-class IkSingleDOFSolutionBase
-{
-public:
-  IkSingleDOFSolutionBase() : fmul(0), foffset(0), freeind(-1), maxsolutions(1)
-  {
-    indices[0] = indices[1] = indices[2] = indices[3] = indices[4] = -1;
-  }
-  T fmul, foffset;             ///< joint value is fmul*sol[freeind]+foffset
-  signed char freeind;         ///< if >= 0, mimics another joint
-  unsigned char jointtype;     ///< joint type, 0x01 is revolute, 0x11 is slider
-  unsigned char maxsolutions;  ///< max possible indices, 0 if controlled by free index or a free joint itself
-  unsigned char indices[5];  ///< unique index of the solution used to keep track on what part it came from. sometimes a
-  /// solution can be repeated for different indices. store at least another repeated root
-};
-
-/// \brief The discrete solutions are returned in this structure.
-///
-/// Sometimes the joint axes of the robot can align allowing an infinite number of solutions.
-/// Stores all these solutions in the form of free variables that the user has to set when querying the solution. Its
-/// prototype is:
-template <typename T>
-class IkSolutionBase
-{
-public:
-  virtual ~IkSolutionBase()
-  {
-  }
-  /// \brief gets a concrete solution
-  ///
-  /// \param[out] solution the result
-  /// \param[in] freevalues values for the free parameters \se GetFree
-  virtual void GetSolution(T* solution, const T* freevalues) const = 0;
-
-  /// \brief std::vector version of \ref GetSolution
-  virtual void GetSolution(std::vector<T>& solution, const std::vector<T>& freevalues) const
-  {
-    solution.resize(GetDOF());
-    GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL);
-  }
-
-  /// \brief Gets the indices of the configuration space that have to be preset before a full solution can be returned
-  ///
-  /// \return vector of indices indicating the free parameters
-  virtual const std::vector<int>& GetFree() const = 0;
-
-  /// \brief the dof of the solution
-  virtual const int GetDOF() const = 0;
-};
-
-/// \brief manages all the solutions
-template <typename T>
-class IkSolutionListBase
-{
-public:
-  virtual ~IkSolutionListBase()
-  {
-  }
-
-  /// \brief add one solution and return its index for later retrieval
-  ///
-  /// \param vinfos Solution data for each degree of freedom of the manipulator
-  /// \param vfree If the solution represents an infinite space, holds free parameters of the solution that users can
-  /// freely set.
-  virtual size_t AddSolution(const std::vector<IkSingleDOFSolutionBase<T> >& vinfos, const std::vector<int>& vfree) = 0;
-
-  /// \brief returns the solution pointer
-  virtual const IkSolutionBase<T>& GetSolution(size_t index) const = 0;
-
-  /// \brief returns the number of solutions stored
-  virtual size_t GetNumSolutions() const = 0;
-
-  /// \brief clears all current solutions, note that any memory addresses returned from \ref GetSolution will be
-  /// invalidated.
-  virtual void Clear() = 0;
-};
-
-/// \brief holds function pointers for all the exported functions of ikfast
-template <typename T>
-class IkFastFunctions
-{
-public:
-  IkFastFunctions()
-    : _ComputeIk(NULL)
-    , _ComputeFk(NULL)
-    , _GetNumFreeParameters(NULL)
-    , _GetFreeParameters(NULL)
-    , _GetNumJoints(NULL)
-    , _GetIkRealSize(NULL)
-    , _GetIkFastVersion(NULL)
-    , _GetIkType(NULL)
-    , _GetKinematicsHash(NULL)
-  {
-  }
-  virtual ~IkFastFunctions()
-  {
-  }
-  typedef bool (*ComputeIkFn)(const T*, const T*, const T*, IkSolutionListBase<T>&);
-  ComputeIkFn _ComputeIk;
-  typedef void (*ComputeFkFn)(const T*, T*, T*);
-  ComputeFkFn _ComputeFk;
-  typedef int (*GetNumFreeParametersFn)();
-  GetNumFreeParametersFn _GetNumFreeParameters;
-  typedef int* (*GetFreeParametersFn)();
-  GetFreeParametersFn _GetFreeParameters;
-  typedef int (*GetNumJointsFn)();
-  GetNumJointsFn _GetNumJoints;
-  typedef int (*GetIkRealSizeFn)();
-  GetIkRealSizeFn _GetIkRealSize;
-  typedef const char* (*GetIkFastVersionFn)();
-  GetIkFastVersionFn _GetIkFastVersion;
-  typedef int (*GetIkTypeFn)();
-  GetIkTypeFn _GetIkType;
-  typedef const char* (*GetKinematicsHashFn)();
-  GetKinematicsHashFn _GetKinematicsHash;
-};
-
-// Implementations of the abstract classes, user doesn't need to use them
-
-/// \brief Default implementation of \ref IkSolutionBase
-template <typename T>
-class IkSolution : public IkSolutionBase<T>
-{
-public:
-  IkSolution(const std::vector<IkSingleDOFSolutionBase<T> >& vinfos, const std::vector<int>& vfree)
-  {
-    _vbasesol = vinfos;
-    _vfree = vfree;
-  }
-
-  virtual void GetSolution(T* solution, const T* freevalues) const
-  {
-    for (std::size_t i = 0; i < _vbasesol.size(); ++i)
-    {
-      if (_vbasesol[i].freeind < 0)
-        solution[i] = _vbasesol[i].foffset;
-      else
-      {
-        solution[i] = freevalues[_vbasesol[i].freeind] * _vbasesol[i].fmul + _vbasesol[i].foffset;
-        if (solution[i] > T(3.14159265358979))
-        {
-          solution[i] -= T(6.28318530717959);
-        }
-        else if (solution[i] < T(-3.14159265358979))
-        {
-          solution[i] += T(6.28318530717959);
-        }
-      }
-    }
-  }
-
-  virtual void GetSolution(std::vector<T>& solution, const std::vector<T>& freevalues) const
-  {
-    solution.resize(GetDOF());
-    GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL);
-  }
-
-  virtual const std::vector<int>& GetFree() const
-  {
-    return _vfree;
-  }
-  virtual const int GetDOF() const
-  {
-    return static_cast<int>(_vbasesol.size());
-  }
-
-  virtual void Validate() const
-  {
-    for (size_t i = 0; i < _vbasesol.size(); ++i)
-    {
-      if (_vbasesol[i].maxsolutions == (unsigned char)-1)
-      {
-        throw std::runtime_error("max solutions for joint not initialized");
-      }
-      if (_vbasesol[i].maxsolutions > 0)
-      {
-        if (_vbasesol[i].indices[0] >= _vbasesol[i].maxsolutions)
-        {
-          throw std::runtime_error("index >= max solutions for joint");
-        }
-        if (_vbasesol[i].indices[1] != (unsigned char)-1 && _vbasesol[i].indices[1] >= _vbasesol[i].maxsolutions)
-        {
-          throw std::runtime_error("2nd index >= max solutions for joint");
-        }
-      }
-    }
-  }
-
-  virtual void GetSolutionIndices(std::vector<unsigned int>& v) const
-  {
-    v.resize(0);
-    v.push_back(0);
-    for (int i = (int)_vbasesol.size() - 1; i >= 0; --i)
-    {
-      if (_vbasesol[i].maxsolutions != (unsigned char)-1 && _vbasesol[i].maxsolutions > 1)
-      {
-        for (size_t j = 0; j < v.size(); ++j)
-        {
-          v[j] *= _vbasesol[i].maxsolutions;
-        }
-        size_t orgsize = v.size();
-        if (_vbasesol[i].indices[1] != (unsigned char)-1)
-        {
-          for (size_t j = 0; j < orgsize; ++j)
-          {
-            v.push_back(v[j] + _vbasesol[i].indices[1]);
-          }
-        }
-        if (_vbasesol[i].indices[0] != (unsigned char)-1)
-        {
-          for (size_t j = 0; j < orgsize; ++j)
-          {
-            v[j] += _vbasesol[i].indices[0];
-          }
-        }
-      }
-    }
-  }
-
-  std::vector<IkSingleDOFSolutionBase<T> > _vbasesol;  ///< solution and their offsets if joints are mimiced
-  std::vector<int> _vfree;
-};
-
-/// \brief Default implementation of \ref IkSolutionListBase
-template <typename T>
-class IkSolutionList : public IkSolutionListBase<T>
-{
-public:
-  virtual size_t AddSolution(const std::vector<IkSingleDOFSolutionBase<T> >& vinfos, const std::vector<int>& vfree)
-  {
-    size_t index = _listsolutions.size();
-    _listsolutions.push_back(IkSolution<T>(vinfos, vfree));
-    return index;
-  }
-
-  virtual const IkSolutionBase<T>& GetSolution(size_t index) const
-  {
-    if (index >= _listsolutions.size())
-    {
-      throw std::runtime_error("GetSolution index is invalid");
-    }
-    typename std::list<IkSolution<T> >::const_iterator it = _listsolutions.begin();
-    std::advance(it, index);
-    return *it;
-  }
-
-  virtual size_t GetNumSolutions() const
-  {
-    return _listsolutions.size();
-  }
-
-  virtual void Clear()
-  {
-    _listsolutions.clear();
-  }
-
-protected:
-  std::list<IkSolution<T> > _listsolutions;
-};
-}
-
-#endif  // OPENRAVE_IKFAST_HEADER
-
-// The following code is dependent on the C++ library linking with.
-#ifdef IKFAST_HAS_LIBRARY
-
-// defined when creating a shared object/dll
-#ifdef IKFAST_CLIBRARY
-#ifdef _MSC_VER
-#define IKFAST_API extern "C" __declspec(dllexport)
-#else
-#define IKFAST_API extern "C"
-#endif
-#else
-#define IKFAST_API
-#endif
-
-#ifdef IKFAST_NAMESPACE
-namespace IKFAST_NAMESPACE
-{
-#endif
-
-#ifdef IKFAST_REAL
-typedef IKFAST_REAL IkReal;
-#else
-typedef double IkReal;
-#endif
-
-/** \brief Computes all IK solutions given a end effector coordinates and the free joints.
-
-   - ``eetrans`` - 3 translation values. For iktype **TranslationXYOrientation3D**, the z-axis is the orientation.
-   - ``eerot``
-   - For **Transform6D** it is 9 values for the 3x3 rotation matrix.
-   - For **Direction3D**, **Ray4D**, and **TranslationDirection5D**, the first 3 values represent the target direction.
-   - For **TranslationXAxisAngle4D**, **TranslationYAxisAngle4D**, and **TranslationZAxisAngle4D** the first value
-   represents the angle.
-   - For **TranslationLocalGlobal6D**, the diagonal elements ([0],[4],[8]) are the local translation inside the end
-   effector coordinate system.
- */
-IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree,
-                          ikfast::IkSolutionListBase<IkReal>& solutions);
-
-/// \brief Computes the end effector coordinates given the joint values. This function is used to double check ik.
-IKFAST_API void ComputeFk(const IkReal* joints, IkReal* eetrans, IkReal* eerot);
-
-/// \brief returns the number of free parameters users has to set apriori
-IKFAST_API int GetNumFreeParameters();
-
-/// \brief the indices of the free parameters indexed by the chain joints
-IKFAST_API int* GetFreeParameters();
-
-/// \brief the total number of indices of the chain
-IKFAST_API int GetNumJoints();
-
-/// \brief the size in bytes of the configured number type
-IKFAST_API int GetIkRealSize();
-
-/// \brief the ikfast version used to generate this file
-IKFAST_API const char* GetIkFastVersion();
-
-/// \brief the ik type ID
-IKFAST_API int GetIkType();
-
-/// \brief a hash of all the chain values used for double checking that the correct IK is used.
-IKFAST_API const char* GetKinematicsHash();
-
-#ifdef IKFAST_NAMESPACE
-}
-#endif
-
-#endif  // IKFAST_HAS_LIBRARY

src/turtlebot_arm/turtlebot_arm_ikfast_plugin/package.xml

@@ -1,32 +0,0 @@
-<?xml version='1.0' encoding='ASCII'?>
-<package>
-  <name>turtlebot_arm_ikfast_plugin</name>
-  <version>0.3.3</version>
-  <description>The turtlebot_arm_ikfast_plugin package</description>
-  <maintainer email="jsantossimon@gmail.com">Jorge Santos</maintainer>
-  <license>BSD</license>
-
-  <url type="website">http://ros.org/wiki/turtlebot_arm_ikfast_plugin</url>
-  <url type="repository">https://github.com/turtlebot/turtlebot_arm</url>
-  <url type="bugtracker">https://github.com/turtlebot/turtlebot_arm/issues</url>
-
-  <buildtool_depend>catkin</buildtool_depend>
-
-  <build_depend>moveit_core</build_depend>
-  <build_depend>liblapack-dev</build_depend>
-  <build_depend>pluginlib</build_depend>
-  <build_depend>roscpp</build_depend>
-  <build_depend>tf_conversions</build_depend>
-  <build_depend>pluginlib</build_depend>
-
-  <run_depend>liblapack-dev</run_depend>
-  <run_depend>moveit_core</run_depend>
-  <run_depend>pluginlib</run_depend>
-  <run_depend>roscpp</run_depend>
-  <run_depend>tf_conversions</run_depend>
-  <run_depend>pluginlib</run_depend>
-
-  <export>
-    <moveit_core plugin="${prefix}/turtlebot_arm_moveit_ikfast_plugin_description.xml"/>
-  </export>
-</package>

src/turtlebot_arm/turtlebot_arm_ikfast_plugin/src/turtlebot_arm_arm_ikfast_moveit_plugin.cpp

@@ -1,1377 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2013, Dave Coleman, CU Boulder; Jeremy Zoss, SwRI; David Butterworth, KAIST; Mathias Lüdtke, Fraunhofer
- *IPA
- * All rights reserved.
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *     * Redistributions of source code must retain the above copyright
- *       notice, this list of conditions and the following disclaimer.
- *     * Redistributions in binary form must reproduce the above copyright
- *       notice, this list of conditions and the following disclaimer in the
- *       documentation and/or other materials provided with the distribution.
- *     * Neither the name of the all of the author's companies nor the names of its
- *       contributors may be used to endorse or promote products derived from
- *       this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- *********************************************************************/
-
-/*
- * IKFast Plugin Template for moveit
- *
- * AUTO-GENERATED by create_ikfast_moveit_plugin.py in arm_kinematics_tools
- * You should run create_ikfast_moveit_plugin.py to generate your own plugin.
- *
- * Creates a kinematics plugin using the output of IKFast from OpenRAVE.
- * This plugin and the move_group node can be used as a general
- * kinematics service, from within the moveit planning environment, or in
- * your own ROS node.
- *
- */
-
-#include <ros/ros.h>
-#include <moveit/kinematics_base/kinematics_base.h>
-#include <urdf/model.h>
-#include <tf_conversions/tf_kdl.h>
-
-// Need a floating point tolerance when checking joint limits, in case the joint starts at limit
-const double LIMIT_TOLERANCE = .0000001;
-/// \brief Search modes for searchPositionIK(), see there
-enum SEARCH_MODE
-{
-  OPTIMIZE_FREE_JOINT = 1,
-  OPTIMIZE_MAX_JOINT = 2
-};
-
-namespace ikfast_kinematics_plugin
-{
-#define IKFAST_NO_MAIN  // Don't include main() from IKFast
-
-/// \brief The types of inverse kinematics parameterizations supported.
-///
-/// The minimum degree of freedoms required is set in the upper 4 bits of each type.
-/// The number of values used to represent the parameterization ( >= dof ) is the next 4 bits.
-/// The lower bits contain a unique id of the type.
-enum IkParameterizationType
-{
-  IKP_None = 0,
-  IKP_Transform6D = 0x67000001,    ///< end effector reaches desired 6D transformation
-  IKP_Rotation3D = 0x34000002,     ///< end effector reaches desired 3D rotation
-  IKP_Translation3D = 0x33000003,  ///< end effector origin reaches desired 3D translation
-  IKP_Direction3D = 0x23000004,    ///< direction on end effector coordinate system reaches desired direction
-  IKP_Ray4D = 0x46000005,          ///< ray on end effector coordinate system reaches desired global ray
-  IKP_Lookat3D = 0x23000006,       ///< direction on end effector coordinate system points to desired 3D position
-  IKP_TranslationDirection5D = 0x56000007,  ///< end effector origin and direction reaches desired 3D translation and
-  /// direction. Can be thought of as Ray IK where the origin of the ray must
-  /// coincide.
-  IKP_TranslationXY2D = 0x22000008,             ///< 2D translation along XY plane
-  IKP_TranslationXYOrientation3D = 0x33000009,  ///< 2D translation along XY plane and 1D rotation around Z axis. The
-  /// offset of the rotation is measured starting at +X, so at +X is it 0,
-  /// at +Y it is pi/2.
-  IKP_TranslationLocalGlobal6D = 0x3600000a,  ///< local point on end effector origin reaches desired 3D global point
-
-  IKP_TranslationXAxisAngle4D = 0x4400000b,  ///< end effector origin reaches desired 3D translation, manipulator
-  /// direction makes a specific angle with x-axis  like a cone, angle is from
-  /// 0-pi. Axes defined in the manipulator base link's coordinate system)
-  IKP_TranslationYAxisAngle4D = 0x4400000c,  ///< end effector origin reaches desired 3D translation, manipulator
-  /// direction makes a specific angle with y-axis  like a cone, angle is from
-  /// 0-pi. Axes defined in the manipulator base link's coordinate system)
-  IKP_TranslationZAxisAngle4D = 0x4400000d,  ///< end effector origin reaches desired 3D translation, manipulator
-  /// direction makes a specific angle with z-axis like a cone, angle is from
-  /// 0-pi. Axes are defined in the manipulator base link's coordinate system.
-
-  IKP_TranslationXAxisAngleZNorm4D = 0x4400000e,  ///< end effector origin reaches desired 3D translation, manipulator
-  /// direction needs to be orthogonal to z-axis and be rotated at a
-  /// certain angle starting from the x-axis (defined in the manipulator
-  /// base link's coordinate system)
-  IKP_TranslationYAxisAngleXNorm4D = 0x4400000f,  ///< end effector origin reaches desired 3D translation, manipulator
-  /// direction needs to be orthogonal to x-axis and be rotated at a
-  /// certain angle starting from the y-axis (defined in the manipulator
-  /// base link's coordinate system)
-  IKP_TranslationZAxisAngleYNorm4D = 0x44000010,  ///< end effector origin reaches desired 3D translation, manipulator
-  /// direction needs to be orthogonal to y-axis and be rotated at a
-  /// certain angle starting from the z-axis (defined in the manipulator
-  /// base link's coordinate system)
-
-  IKP_NumberOfParameterizations = 16,  ///< number of parameterizations (does not count IKP_None)
-
-  IKP_VelocityDataBit =
-      0x00008000,  ///< bit is set if the data represents the time-derivate velocity of an IkParameterization
-  IKP_Transform6DVelocity = IKP_Transform6D | IKP_VelocityDataBit,
-  IKP_Rotation3DVelocity = IKP_Rotation3D | IKP_VelocityDataBit,
-  IKP_Translation3DVelocity = IKP_Translation3D | IKP_VelocityDataBit,
-  IKP_Direction3DVelocity = IKP_Direction3D | IKP_VelocityDataBit,
-  IKP_Ray4DVelocity = IKP_Ray4D | IKP_VelocityDataBit,
-  IKP_Lookat3DVelocity = IKP_Lookat3D | IKP_VelocityDataBit,
-  IKP_TranslationDirection5DVelocity = IKP_TranslationDirection5D | IKP_VelocityDataBit,
-  IKP_TranslationXY2DVelocity = IKP_TranslationXY2D | IKP_VelocityDataBit,
-  IKP_TranslationXYOrientation3DVelocity = IKP_TranslationXYOrientation3D | IKP_VelocityDataBit,
-  IKP_TranslationLocalGlobal6DVelocity = IKP_TranslationLocalGlobal6D | IKP_VelocityDataBit,
-  IKP_TranslationXAxisAngle4DVelocity = IKP_TranslationXAxisAngle4D | IKP_VelocityDataBit,
-  IKP_TranslationYAxisAngle4DVelocity = IKP_TranslationYAxisAngle4D | IKP_VelocityDataBit,
-  IKP_TranslationZAxisAngle4DVelocity = IKP_TranslationZAxisAngle4D | IKP_VelocityDataBit,
-  IKP_TranslationXAxisAngleZNorm4DVelocity = IKP_TranslationXAxisAngleZNorm4D | IKP_VelocityDataBit,
-  IKP_TranslationYAxisAngleXNorm4DVelocity = IKP_TranslationYAxisAngleXNorm4D | IKP_VelocityDataBit,
-  IKP_TranslationZAxisAngleYNorm4DVelocity = IKP_TranslationZAxisAngleYNorm4D | IKP_VelocityDataBit,
-
-  IKP_UniqueIdMask = 0x0000ffff,   ///< the mask for the unique ids
-  IKP_CustomDataBit = 0x00010000,  ///< bit is set if the ikparameterization contains custom data, this is only used
-  /// when serializing the ik parameterizations
-};
-
-// struct for storing and sorting solutions
-struct LimitObeyingSol
-{
-  std::vector<double> value;
-  double dist_from_seed;
-
-  bool operator<(const LimitObeyingSol& a) const
-  {
-    return dist_from_seed < a.dist_from_seed;
-  }
-};
-
-// Code generated by IKFast56/61
-#include "turtlebot_arm_arm_ikfast_solver.cpp"
-
-class IKFastKinematicsPlugin : public kinematics::KinematicsBase
-{
-  std::vector<std::string> joint_names_;
-  std::vector<double> joint_min_vector_;
-  std::vector<double> joint_max_vector_;
-  std::vector<bool> joint_has_limits_vector_;
-  std::vector<std::string> link_names_;
-  const size_t num_joints_;
-  std::vector<int> free_params_;
-  bool active_;  // Internal variable that indicates whether solvers are configured and ready
-  const std::string name_{ "ikfast" };
-
-  const std::vector<std::string>& getJointNames() const
-  {
-    return joint_names_;
-  }
-  const std::vector<std::string>& getLinkNames() const
-  {
-    return link_names_;
-  }
-
-public:
-  /** @class
-   *  @brief Interface for an IKFast kinematics plugin
-   */
-  IKFastKinematicsPlugin() : num_joints_(GetNumJoints()), active_(false)
-  {
-    srand(time(NULL));
-    supported_methods_.push_back(kinematics::DiscretizationMethods::NO_DISCRETIZATION);
-    supported_methods_.push_back(kinematics::DiscretizationMethods::ALL_DISCRETIZED);
-    supported_methods_.push_back(kinematics::DiscretizationMethods::ALL_RANDOM_SAMPLED);
-  }
-
-  /**
-   * @brief Given a desired pose of the end-effector, compute the joint angles to reach it
-   * @param ik_pose the desired pose of the link
-   * @param ik_seed_state an initial guess solution for the inverse kinematics
-   * @param solution the solution vector
-   * @param error_code an error code that encodes the reason for failure or success
-   * @return True if a valid solution was found, false otherwise
-   */
-
-  // Returns the IK solution that is within joint limits closest to ik_seed_state
-  bool getPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
-                     std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
-                     const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const;
-
-  /**
-   * @brief Given a desired pose of the end-effector, compute the set joint angles solutions that are able to reach it.
-   *
-   * This is a default implementation that returns only one solution and so its result is equivalent to calling
-   * 'getPositionIK(...)' with a zero initialized seed.
-   *
-   * @param ik_poses  The desired pose of each tip link
-   * @param ik_seed_state an initial guess solution for the inverse kinematics
-   * @param solutions A vector of vectors where each entry is a valid joint solution
-   * @param result A struct that reports the results of the query
-   * @param options An option struct which contains the type of redundancy discretization used. This default
-   *                implementation only supports the KinmaticSearches::NO_DISCRETIZATION method; requesting any
-   *                other will result in failure.
-   * @return True if a valid set of solutions was found, false otherwise.
-   */
-  bool getPositionIK(const std::vector<geometry_msgs::Pose>& ik_poses, const std::vector<double>& ik_seed_state,
-                     std::vector<std::vector<double>>& solutions, kinematics::KinematicsResult& result,
-                     const kinematics::KinematicsQueryOptions& options) const;
-
-  /**
-   * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it.
-   * This particular method is intended for "searching" for a solutions by stepping through the redundancy
-   * (or other numerical routines).
-   * @param ik_pose the desired pose of the link
-   * @param ik_seed_state an initial guess solution for the inverse kinematics
-   * @return True if a valid solution was found, false otherwise
-   */
-  bool searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
-                        std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
-                        const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const;
-
-  /**
-   * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it.
-   * This particular method is intended for "searching" for a solutions by stepping through the redundancy
-   * (or other numerical routines).
-   * @param ik_pose the desired pose of the link
-   * @param ik_seed_state an initial guess solution for the inverse kinematics
-   * @param the distance that the redundancy can be from the current position
-   * @return True if a valid solution was found, false otherwise
-   */
-  bool searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
-                        const std::vector<double>& consistency_limits, std::vector<double>& solution,
-                        moveit_msgs::MoveItErrorCodes& error_code,
-                        const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const;
-
-  /**
-   * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it.
-   * This particular method is intended for "searching" for a solutions by stepping through the redundancy
-   * (or other numerical routines).
-   * @param ik_pose the desired pose of the link
-   * @param ik_seed_state an initial guess solution for the inverse kinematics
-   * @return True if a valid solution was found, false otherwise
-   */
-  bool searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
-                        std::vector<double>& solution, const IKCallbackFn& solution_callback,
-                        moveit_msgs::MoveItErrorCodes& error_code,
-                        const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const;
-
-  /**
-   * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it.
-   * This particular method is intended for "searching" for a solutions by stepping through the redundancy
-   * (or other numerical routines).  The consistency_limit specifies that only certain redundancy positions
-   * around those specified in the seed state are admissible and need to be searched.
-   * @param ik_pose the desired pose of the link
-   * @param ik_seed_state an initial guess solution for the inverse kinematics
-   * @param consistency_limit the distance that the redundancy can be from the current position
-   * @return True if a valid solution was found, false otherwise
-   */
-  bool searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
-                        const std::vector<double>& consistency_limits, std::vector<double>& solution,
-                        const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code,
-                        const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const;
-
-  /**
-   * @brief Given a set of joint angles and a set of links, compute their pose
-   *
-   * @param link_names A set of links for which FK needs to be computed
-   * @param joint_angles The state for which FK is being computed
-   * @param poses The resultant set of poses (in the frame returned by getBaseFrame())
-   * @return True if a valid solution was found, false otherwise
-   */
-  bool getPositionFK(const std::vector<std::string>& link_names, const std::vector<double>& joint_angles,
-                     std::vector<geometry_msgs::Pose>& poses) const;
-
-  /**
-   * @brief Sets the discretization value for the redundant joint.
-   *
-   * Since this ikfast implementation allows for one redundant joint then only the first entry will be in the
-   *discretization map will be used.
-   * Calling this method replaces previous discretization settings.
-   *
-   * @param discretization a map of joint indices and discretization value pairs.
-   */
-  void setSearchDiscretization(const std::map<int, double>& discretization);
-
-  /**
-   * @brief Overrides the default method to prevent changing the redundant joints
-   */
-  bool setRedundantJoints(const std::vector<unsigned int>& redundant_joint_indices);
-
-private:
-  bool initialize(const std::string& robot_description, const std::string& group_name, const std::string& base_name,
-                  const std::string& tip_name, double search_discretization);
-
-  /**
-   * @brief Calls the IK solver from IKFast
-   * @return The number of solutions found
-   */
-  int solve(KDL::Frame& pose_frame, const std::vector<double>& vfree, IkSolutionList<IkReal>& solutions) const;
-
-  /**
-   * @brief Gets a specific solution from the set
-   */
-  void getSolution(const IkSolutionList<IkReal>& solutions, int i, std::vector<double>& solution) const;
-
-  /**
-   * @brief Gets a specific solution from the set with joints rotated 360° to be near seed state where possible
-   */
-  void getSolution(const IkSolutionList<IkReal>& solutions, const std::vector<double>& ik_seed_state, int i,
-                   std::vector<double>& solution) const;
-
-  double harmonize(const std::vector<double>& ik_seed_state, std::vector<double>& solution) const;
-  // void getOrderedSolutions(const std::vector<double> &ik_seed_state, std::vector<std::vector<double> >& solslist);
-  void getClosestSolution(const IkSolutionList<IkReal>& solutions, const std::vector<double>& ik_seed_state,
-                          std::vector<double>& solution) const;
-  void fillFreeParams(int count, int* array);
-  bool getCount(int& count, const int& max_count, const int& min_count) const;
-
-  /**
-  * @brief samples the designated redundant joint using the chosen discretization method
-  * @param  method              An enumeration flag indicating the discretization method to be used
-  * @param  sampled_joint_vals  Sampled joint values for the redundant joint
-  * @return True if sampling succeeded.
-  */
-  bool sampleRedundantJoint(kinematics::DiscretizationMethod method, std::vector<double>& sampled_joint_vals) const;
-
-};  // end class
-
-bool IKFastKinematicsPlugin::initialize(const std::string& robot_description, const std::string& group_name,
-                                        const std::string& base_name, const std::string& tip_name,
-                                        double search_discretization)
-{
-  setValues(robot_description, group_name, base_name, tip_name, search_discretization);
-
-  ros::NodeHandle node_handle("~/" + group_name);
-
-  std::string robot;
-  lookupParam("robot", robot, std::string());
-
-  // IKFast56/61
-  fillFreeParams(GetNumFreeParameters(), GetFreeParameters());
-
-  if (free_params_.size() > 1)
-  {
-    ROS_FATAL("Only one free joint parameter supported!");
-    return false;
-  }
-  else if (free_params_.size() == 1)
-  {
-    redundant_joint_indices_.clear();
-    redundant_joint_indices_.push_back(free_params_[0]);
-    KinematicsBase::setSearchDiscretization(DEFAULT_SEARCH_DISCRETIZATION);
-  }
-
-  urdf::Model robot_model;
-  std::string xml_string;
-
-  std::string urdf_xml, full_urdf_xml;
-  lookupParam("urdf_xml", urdf_xml, robot_description);
-  node_handle.searchParam(urdf_xml, full_urdf_xml);
-
-  ROS_DEBUG_NAMED(name_, "Reading xml file from parameter server");
-  if (!node_handle.getParam(full_urdf_xml, xml_string))
-  {
-    ROS_FATAL_NAMED(name_, "Could not load the xml from parameter server: %s", urdf_xml.c_str());
-    return false;
-  }
-
-  robot_model.initString(xml_string);
-
-  ROS_DEBUG_STREAM_NAMED(name_, "Reading joints and links from URDF");
-
-  urdf::LinkConstSharedPtr link = robot_model.getLink(getTipFrame());
-  while (link->name != base_frame_ && joint_names_.size() <= num_joints_)
-  {
-    ROS_DEBUG_NAMED(name_, "Link %s", link->name.c_str());
-    link_names_.push_back(link->name);
-    urdf::JointSharedPtr joint = link->parent_joint;
-    if (joint)
-    {
-      if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
-      {
-        ROS_DEBUG_STREAM_NAMED(name_, "Adding joint " << joint->name);
-
-        joint_names_.push_back(joint->name);
-        float lower, upper;
-        int hasLimits;
-        if (joint->type != urdf::Joint::CONTINUOUS)
-        {
-          if (joint->safety)
-          {
-            lower = joint->safety->soft_lower_limit;
-            upper = joint->safety->soft_upper_limit;
-          }
-          else
-          {
-            lower = joint->limits->lower;
-            upper = joint->limits->upper;
-          }
-          hasLimits = 1;
-        }
-        else
-        {
-          lower = -M_PI;
-          upper = M_PI;
-          hasLimits = 0;
-        }
-        if (hasLimits)
-        {
-          joint_has_limits_vector_.push_back(true);
-          joint_min_vector_.push_back(lower);
-          joint_max_vector_.push_back(upper);
-        }
-        else
-        {
-          joint_has_limits_vector_.push_back(false);
-          joint_min_vector_.push_back(-M_PI);
-          joint_max_vector_.push_back(M_PI);
-        }
-      }
-    }
-    else
-    {
-      ROS_WARN_NAMED(name_, "no joint corresponding to %s", link->name.c_str());
-    }
-    link = link->getParent();
-  }
-
-  if (joint_names_.size() != num_joints_)
-  {
-    ROS_FATAL_STREAM_NAMED(name_, "Joint numbers mismatch: URDF has " << joint_names_.size() << " and IKFast has "
-                                                                      << num_joints_);
-    return false;
-  }
-
-  std::reverse(link_names_.begin(), link_names_.end());
-  std::reverse(joint_names_.begin(), joint_names_.end());
-  std::reverse(joint_min_vector_.begin(), joint_min_vector_.end());
-  std::reverse(joint_max_vector_.begin(), joint_max_vector_.end());
-  std::reverse(joint_has_limits_vector_.begin(), joint_has_limits_vector_.end());
-
-  for (size_t i = 0; i < num_joints_; ++i)
-    ROS_DEBUG_STREAM_NAMED(name_, joint_names_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i] << " "
-                                                  << joint_has_limits_vector_[i]);
-
-  active_ = true;
-  return true;
-}
-
-void IKFastKinematicsPlugin::setSearchDiscretization(const std::map<int, double>& discretization)
-{
-  if (discretization.empty())
-  {
-    ROS_ERROR("The 'discretization' map is empty");
-    return;
-  }
-
-  if (redundant_joint_indices_.empty())
-  {
-    ROS_ERROR_STREAM("This group's solver doesn't support redundant joints");
-    return;
-  }
-
-  if (discretization.begin()->first != redundant_joint_indices_[0])
-  {
-    std::string redundant_joint = joint_names_[free_params_[0]];
-    ROS_ERROR_STREAM("Attempted to discretize a non-redundant joint "
-                     << discretization.begin()->first << ", only joint '" << redundant_joint << "' with index "
-                     << redundant_joint_indices_[0] << " is redundant.");
-    return;
-  }
-
-  if (discretization.begin()->second <= 0.0)
-  {
-    ROS_ERROR_STREAM("Discretization can not takes values that are <= 0");
-    return;
-  }
-
-  redundant_joint_discretization_.clear();
-  redundant_joint_discretization_[redundant_joint_indices_[0]] = discretization.begin()->second;
-}
-
-bool IKFastKinematicsPlugin::setRedundantJoints(const std::vector<unsigned int>& redundant_joint_indices)
-{
-  ROS_ERROR_STREAM("Changing the redundant joints isn't permitted by this group's solver ");
-  return false;
-}
-
-int IKFastKinematicsPlugin::solve(KDL::Frame& pose_frame, const std::vector<double>& vfree,
-                                  IkSolutionList<IkReal>& solutions) const
-{
-  // IKFast56/61
-  solutions.Clear();
-
-  double trans[3];
-  trans[0] = pose_frame.p[0];  //-.18;
-  trans[1] = pose_frame.p[1];
-  trans[2] = pose_frame.p[2];
-
-  KDL::Rotation mult;
-  KDL::Vector direction;
-
-  switch (GetIkType())
-  {
-    case IKP_Transform6D:
-    case IKP_Translation3D:
-      // For **Transform6D**, eerot is 9 values for the 3x3 rotation matrix. For **Translation3D**, these are ignored.
-
-      mult = pose_frame.M;
-
-      double vals[9];
-      vals[0] = mult(0, 0);
-      vals[1] = mult(0, 1);
-      vals[2] = mult(0, 2);
-      vals[3] = mult(1, 0);
-      vals[4] = mult(1, 1);
-      vals[5] = mult(1, 2);
-      vals[6] = mult(2, 0);
-      vals[7] = mult(2, 1);
-      vals[8] = mult(2, 2);
-
-      // IKFast56/61
-      ComputeIk(trans, vals, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
-      return solutions.GetNumSolutions();
-
-    case IKP_Direction3D:
-    case IKP_Ray4D:
-    case IKP_TranslationDirection5D:
-      // For **Direction3D**, **Ray4D**, and **TranslationDirection5D**, the first 3 values represent the target
-      // direction.
-
-      direction = pose_frame.M * KDL::Vector(0, 0, 1);
-      ComputeIk(trans, direction.data, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
-      return solutions.GetNumSolutions();
-
-    case IKP_TranslationXAxisAngle4D:
-    case IKP_TranslationYAxisAngle4D:
-    case IKP_TranslationZAxisAngle4D:
-      // For **TranslationXAxisAngle4D**, **TranslationYAxisAngle4D**, and **TranslationZAxisAngle4D**, the first value
-      // represents the angle.
-      ROS_ERROR_NAMED(name_, "IK for this IkParameterizationType not implemented yet.");
-      return 0;
-
-    case IKP_TranslationLocalGlobal6D:
-      // For **TranslationLocalGlobal6D**, the diagonal elements ([0],[4],[8]) are the local translation inside the end
-      // effector coordinate system.
-      ROS_ERROR_NAMED(name_, "IK for this IkParameterizationType not implemented yet.");
-      return 0;
-
-    case IKP_Rotation3D:
-    case IKP_Lookat3D:
-    case IKP_TranslationXY2D:
-    case IKP_TranslationXYOrientation3D:
-    case IKP_TranslationXAxisAngleZNorm4D:
-    case IKP_TranslationYAxisAngleXNorm4D:
-    case IKP_TranslationZAxisAngleYNorm4D:
-      ROS_ERROR_NAMED(name_, "IK for this IkParameterizationType not implemented yet.");
-      return 0;
-
-    default:
-      ROS_ERROR_NAMED(name_, "Unknown IkParameterizationType! Was the solver generated with an incompatible version "
-                             "of Openrave?");
-      return 0;
-  }
-}
-
-void IKFastKinematicsPlugin::getSolution(const IkSolutionList<IkReal>& solutions, int i,
-                                         std::vector<double>& solution) const
-{
-  solution.clear();
-  solution.resize(num_joints_);
-
-  // IKFast56/61
-  const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
-  std::vector<IkReal> vsolfree(sol.GetFree().size());
-  sol.GetSolution(&solution[0], vsolfree.size() > 0 ? &vsolfree[0] : NULL);
-
-  // std::cout << "solution " << i << ":" ;
-  // for(int j=0;j<num_joints_; ++j)
-  //   std::cout << " " << solution[j];
-  // std::cout << std::endl;
-
-  // ROS_ERROR("%f %d",solution[2],vsolfree.size());
-}
-
-void IKFastKinematicsPlugin::getSolution(const IkSolutionList<IkReal>& solutions,
-                                         const std::vector<double>& ik_seed_state, int i,
-                                         std::vector<double>& solution) const
-{
-  solution.clear();
-  solution.resize(num_joints_);
-
-  // IKFast56/61
-  const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
-  std::vector<IkReal> vsolfree(sol.GetFree().size());
-  sol.GetSolution(&solution[0], vsolfree.size() > 0 ? &vsolfree[0] : NULL);
-
-  // rotate joints by +/-360° where it is possible and useful
-  for (std::size_t i = 0; i < num_joints_; ++i)
-  {
-    if (joint_has_limits_vector_[i])
-    {
-      double signed_distance = solution[i] - ik_seed_state[i];
-      while (signed_distance > M_PI && solution[i] - 2 * M_PI > (joint_min_vector_[i] - LIMIT_TOLERANCE))
-      {
-        signed_distance -= 2 * M_PI;
-        solution[i] -= 2 * M_PI;
-      }
-      while (signed_distance < -M_PI && solution[i] + 2 * M_PI < (joint_max_vector_[i] + LIMIT_TOLERANCE))
-      {
-        signed_distance += 2 * M_PI;
-        solution[i] += 2 * M_PI;
-      }
-    }
-  }
-}
-
-double IKFastKinematicsPlugin::harmonize(const std::vector<double>& ik_seed_state, std::vector<double>& solution) const
-{
-  double dist_sqr = 0;
-  std::vector<double> ss = ik_seed_state;
-  for (size_t i = 0; i < ik_seed_state.size(); ++i)
-  {
-    while (ss[i] > 2 * M_PI)
-    {
-      ss[i] -= 2 * M_PI;
-    }
-    while (ss[i] < 2 * M_PI)
-    {
-      ss[i] += 2 * M_PI;
-    }
-    while (solution[i] > 2 * M_PI)
-    {
-      solution[i] -= 2 * M_PI;
-    }
-    while (solution[i] < 2 * M_PI)
-    {
-      solution[i] += 2 * M_PI;
-    }
-    dist_sqr += fabs(ik_seed_state[i] - solution[i]);
-  }
-  return dist_sqr;
-}
-
-// void IKFastKinematicsPlugin::getOrderedSolutions(const std::vector<double> &ik_seed_state,
-//                                  std::vector<std::vector<double> >& solslist)
-// {
-//   std::vector<double>
-//   double mindist = 0;
-//   int minindex = -1;
-//   std::vector<double> sol;
-//   for(size_t i=0;i<solslist.size();++i){
-//     getSolution(i,sol);
-//     double dist = harmonize(ik_seed_state, sol);
-//     //std::cout << "dist[" << i << "]= " << dist << std::endl;
-//     if(minindex == -1 || dist<mindist){
-//       minindex = i;
-//       mindist = dist;
-//     }
-//   }
-//   if(minindex >= 0){
-//     getSolution(minindex,solution);
-//     harmonize(ik_seed_state, solution);
-//     index = minindex;
-//   }
-// }
-
-void IKFastKinematicsPlugin::getClosestSolution(const IkSolutionList<IkReal>& solutions,
-                                                const std::vector<double>& ik_seed_state,
-                                                std::vector<double>& solution) const
-{
-  double mindist = DBL_MAX;
-  int minindex = -1;
-  std::vector<double> sol;
-
-  // IKFast56/61
-  for (size_t i = 0; i < solutions.GetNumSolutions(); ++i)
-  {
-    getSolution(solutions, i, sol);
-    double dist = harmonize(ik_seed_state, sol);
-    ROS_INFO_STREAM_NAMED(name_, "Dist " << i << " dist " << dist);
-    // std::cout << "dist[" << i << "]= " << dist << std::endl;
-    if (minindex == -1 || dist < mindist)
-    {
-      minindex = i;
-      mindist = dist;
-    }
-  }
-  if (minindex >= 0)
-  {
-    getSolution(solutions, minindex, solution);
-    harmonize(ik_seed_state, solution);
-  }
-}
-
-void IKFastKinematicsPlugin::fillFreeParams(int count, int* array)
-{
-  free_params_.clear();
-  for (int i = 0; i < count; ++i)
-    free_params_.push_back(array[i]);
-}
-
-bool IKFastKinematicsPlugin::getCount(int& count, const int& max_count, const int& min_count) const
-{
-  if (count > 0)
-  {
-    if (-count >= min_count)
-    {
-      count = -count;
-      return true;
-    }
-    else if (count + 1 <= max_count)
-    {
-      count = count + 1;
-      return true;
-    }
-    else
-    {
-      return false;
-    }
-  }
-  else
-  {
-    if (1 - count <= max_count)
-    {
-      count = 1 - count;
-      return true;
-    }
-    else if (count - 1 >= min_count)
-    {
-      count = count - 1;
-      return true;
-    }
-    else
-      return false;
-  }
-}
-
-bool IKFastKinematicsPlugin::getPositionFK(const std::vector<std::string>& link_names,
-                                           const std::vector<double>& joint_angles,
-                                           std::vector<geometry_msgs::Pose>& poses) const
-{
-  if (GetIkType() != IKP_Transform6D)
-  {
-    // ComputeFk() is the inverse function of ComputeIk(), so the format of
-    // eerot differs depending on IK type. The Transform6D IK type is the only
-    // one for which a 3x3 rotation matrix is returned, which means we can only
-    // compute FK for that IK type.
-    ROS_ERROR_NAMED(name_, "Can only compute FK for Transform6D IK type!");
-    return false;
-  }
-
-  KDL::Frame p_out;
-  if (link_names.size() == 0)
-  {
-    ROS_WARN_STREAM_NAMED(name_, "Link names with nothing");
-    return false;
-  }
-
-  if (link_names.size() != 1 || link_names[0] != getTipFrame())
-  {
-    ROS_ERROR_NAMED(name_, "Can compute FK for %s only", getTipFrame().c_str());
-    return false;
-  }
-
-  bool valid = true;
-
-  IkReal eerot[9], eetrans[3];
-
-  if (joint_angles.size() != num_joints_)
-  {
-    ROS_ERROR_NAMED(name_, "Unexpected number of joint angles");
-    return false;
-  }
-
-  IkReal angles[num_joints_];
-  for (unsigned char i = 0; i < num_joints_; i++)
-    angles[i] = joint_angles[i];
-
-  // IKFast56/61
-  ComputeFk(angles, eetrans, eerot);
-
-  for (int i = 0; i < 3; ++i)
-    p_out.p.data[i] = eetrans[i];
-
-  for (int i = 0; i < 9; ++i)
-    p_out.M.data[i] = eerot[i];
-
-  poses.resize(1);
-  tf::poseKDLToMsg(p_out, poses[0]);
-
-  return valid;
-}
-
-bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose,
-                                              const std::vector<double>& ik_seed_state, double timeout,
-                                              std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
-                                              const kinematics::KinematicsQueryOptions& options) const
-{
-  const IKCallbackFn solution_callback = 0;
-  std::vector<double> consistency_limits;
-
-  return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code,
-                          options);
-}
-
-bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose,
-                                              const std::vector<double>& ik_seed_state, double timeout,
-                                              const std::vector<double>& consistency_limits,
-                                              std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
-                                              const kinematics::KinematicsQueryOptions& options) const
-{
-  const IKCallbackFn solution_callback = 0;
-  return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code,
-                          options);
-}
-
-bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose,
-                                              const std::vector<double>& ik_seed_state, double timeout,
-                                              std::vector<double>& solution, const IKCallbackFn& solution_callback,
-                                              moveit_msgs::MoveItErrorCodes& error_code,
-                                              const kinematics::KinematicsQueryOptions& options) const
-{
-  std::vector<double> consistency_limits;
-  return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code,
-                          options);
-}
-
-bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose,
-                                              const std::vector<double>& ik_seed_state, double timeout,
-                                              const std::vector<double>& consistency_limits,
-                                              std::vector<double>& solution, const IKCallbackFn& solution_callback,
-                                              moveit_msgs::MoveItErrorCodes& error_code,
-                                              const kinematics::KinematicsQueryOptions& options) const
-{
-  ROS_DEBUG_STREAM_NAMED(name_, "searchPositionIK");
-
-  /// search_mode is currently fixed during code generation
-  SEARCH_MODE search_mode = OPTIMIZE_MAX_JOINT;
-
-  // Check if there are no redundant joints
-  if (free_params_.size() == 0)
-  {
-    ROS_DEBUG_STREAM_NAMED(name_, "No need to search since no free params/redundant joints");
-
-    std::vector<geometry_msgs::Pose> ik_poses(1, ik_pose);
-    std::vector<std::vector<double>> solutions;
-    kinematics::KinematicsResult kinematic_result;
-    // Find all IK solution within joint limits
-    if (!getPositionIK(ik_poses, ik_seed_state, solutions, kinematic_result, options))
-    {
-      ROS_DEBUG_STREAM_NAMED(name_, "No solution whatsoever");
-      error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
-      return false;
-    }
-
-    // sort solutions by their distance to the seed
-    std::vector<LimitObeyingSol> solutions_obey_limits;
-    for (std::size_t i = 0; i < solutions.size(); ++i)
-    {
-      double dist_from_seed = 0.0;
-      for (std::size_t j = 0; j < ik_seed_state.size(); ++j)
-      {
-        dist_from_seed += fabs(ik_seed_state[j] - solutions[i][j]);
-      }
-
-      solutions_obey_limits.push_back({ solutions[i], dist_from_seed });
-    }
-    std::sort(solutions_obey_limits.begin(), solutions_obey_limits.end());
-
-    // check for collisions if a callback is provided
-    if (!solution_callback.empty())
-    {
-      for (std::size_t i = 0; i < solutions_obey_limits.size(); ++i)
-      {
-        solution_callback(ik_pose, solutions_obey_limits[i].value, error_code);
-        if (error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
-        {
-          solution = solutions_obey_limits[i].value;
-          ROS_DEBUG_STREAM_NAMED(name_, "Solution passes callback");
-          return true;
-        }
-      }
-
-      ROS_DEBUG_STREAM_NAMED(name_, "Solution has error code " << error_code);
-      return false;
-    }
-    else
-    {
-      solution = solutions_obey_limits[0].value;
-      error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
-      return true;  // no collision check callback provided
-    }
-  }
-
-  // -------------------------------------------------------------------------------------------------
-  // Error Checking
-  if (!active_)
-  {
-    ROS_ERROR_STREAM_NAMED(name_, "Kinematics not active");
-    error_code.val = error_code.NO_IK_SOLUTION;
-    return false;
-  }
-
-  if (ik_seed_state.size() != num_joints_)
-  {
-    ROS_ERROR_STREAM_NAMED(name_, "Seed state must have size " << num_joints_ << " instead of size "
-                                                               << ik_seed_state.size());
-    error_code.val = error_code.NO_IK_SOLUTION;
-    return false;
-  }
-
-  if (!consistency_limits.empty() && consistency_limits.size() != num_joints_)
-  {
-    ROS_ERROR_STREAM_NAMED(name_, "Consistency limits be empty or must have size " << num_joints_ << " instead of size "
-                                                                                   << consistency_limits.size());
-    error_code.val = error_code.NO_IK_SOLUTION;
-    return false;
-  }
-
-  // -------------------------------------------------------------------------------------------------
-  // Initialize
-
-  KDL::Frame frame;
-  tf::poseMsgToKDL(ik_pose, frame);
-
-  std::vector<double> vfree(free_params_.size());
-
-  ros::Time maxTime = ros::Time::now() + ros::Duration(timeout);
-  int counter = 0;
-
-  double initial_guess = ik_seed_state[free_params_[0]];
-  vfree[0] = initial_guess;
-
-  // -------------------------------------------------------------------------------------------------
-  // Handle consitency limits if needed
-  int num_positive_increments;
-  int num_negative_increments;
-
-  if (!consistency_limits.empty())
-  {
-    // moveit replaced consistency_limit (scalar) w/ consistency_limits (vector)
-    // Assume [0]th free_params element for now.  Probably wrong.
-    double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess + consistency_limits[free_params_[0]]);
-    double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess - consistency_limits[free_params_[0]]);
-
-    num_positive_increments = (int)((max_limit - initial_guess) / search_discretization_);
-    num_negative_increments = (int)((initial_guess - min_limit) / search_discretization_);
-  }
-  else  // no consitency limits provided
-  {
-    num_positive_increments = (joint_max_vector_[free_params_[0]] - initial_guess) / search_discretization_;
-    num_negative_increments = (initial_guess - joint_min_vector_[free_params_[0]]) / search_discretization_;
-  }
-
-  // -------------------------------------------------------------------------------------------------
-  // Begin searching
-
-  ROS_DEBUG_STREAM_NAMED(name_, "Free param is " << free_params_[0] << " initial guess is " << initial_guess
-                                                 << ", # positive increments: " << num_positive_increments
-                                                 << ", # negative increments: " << num_negative_increments);
-  if ((search_mode & OPTIMIZE_MAX_JOINT) && (num_positive_increments + num_negative_increments) > 1000)
-    ROS_WARN_STREAM_ONCE_NAMED(name_, "Large search space, consider increasing the search discretization");
-
-  double best_costs = -1.0;
-  std::vector<double> best_solution;
-  int nattempts = 0, nvalid = 0;
-
-  while (true)
-  {
-    IkSolutionList<IkReal> solutions;
-    int numsol = solve(frame, vfree, solutions);
-
-    ROS_DEBUG_STREAM_NAMED(name_, "Found " << numsol << " solutions from IKFast");
-
-    // ROS_INFO("%f",vfree[0]);
-
-    if (numsol > 0)
-    {
-      for (int s = 0; s < numsol; ++s)
-      {
-        nattempts++;
-        std::vector<double> sol;
-        getSolution(solutions, ik_seed_state, s, sol);
-
-        bool obeys_limits = true;
-        for (unsigned int i = 0; i < sol.size(); i++)
-        {
-          if (joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i]))
-          {
-            obeys_limits = false;
-            break;
-          }
-          // ROS_INFO_STREAM_NAMED(name_,"Num " << i << " value " << sol[i] << " has limits " <<
-          // joint_has_limits_vector_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i]);
-        }
-        if (obeys_limits)
-        {
-          getSolution(solutions, ik_seed_state, s, solution);
-
-          // This solution is within joint limits, now check if in collision (if callback provided)
-          if (!solution_callback.empty())
-          {
-            solution_callback(ik_pose, solution, error_code);
-          }
-          else
-          {
-            error_code.val = error_code.SUCCESS;
-          }
-
-          if (error_code.val == error_code.SUCCESS)
-          {
-            nvalid++;
-            if (search_mode & OPTIMIZE_MAX_JOINT)
-            {
-              // Costs for solution: Largest joint motion
-              double costs = 0.0;
-              for (unsigned int i = 0; i < solution.size(); i++)
-              {
-                double d = fabs(ik_seed_state[i] - solution[i]);
-                if (d > costs)
-                  costs = d;
-              }
-              if (costs < best_costs || best_costs == -1.0)
-              {
-                best_costs = costs;
-                best_solution = solution;
-              }
-            }
-            else
-              // Return first feasible solution
-              return true;
-          }
-        }
-      }
-    }
-
-    if (!getCount(counter, num_positive_increments, -num_negative_increments))
-    {
-      // Everything searched
-      error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
-      break;
-    }
-
-    vfree[0] = initial_guess + search_discretization_ * counter;
-    // ROS_DEBUG_STREAM_NAMED(name_,"Attempt " << counter << " with 0th free joint having value " << vfree[0]);
-  }
-
-  ROS_DEBUG_STREAM_NAMED(name_, "Valid solutions: " << nvalid << "/" << nattempts);
-
-  if ((search_mode & OPTIMIZE_MAX_JOINT) && best_costs != -1.0)
-  {
-    solution = best_solution;
-    error_code.val = error_code.SUCCESS;
-    return true;
-  }
-
-  // No solution found
-  error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
-  return false;
-}
-
-// Used when there are no redundant joints - aka no free params
-bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
-                                           std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
-                                           const kinematics::KinematicsQueryOptions& options) const
-{
-  ROS_DEBUG_STREAM_NAMED(name_, "getPositionIK");
-
-  if (!active_)
-  {
-    ROS_ERROR("kinematics not active");
-    return false;
-  }
-
-  if (ik_seed_state.size() < num_joints_)
-  {
-    ROS_ERROR_STREAM("ik_seed_state only has " << ik_seed_state.size() << " entries, this ikfast solver requires "
-                                               << num_joints_);
-    return false;
-  }
-
-  // Check if seed is in bound
-  for (std::size_t i = 0; i < ik_seed_state.size(); i++)
-  {
-    // Add tolerance to limit check
-    if (joint_has_limits_vector_[i] && ((ik_seed_state[i] < (joint_min_vector_[i] - LIMIT_TOLERANCE)) ||
-                                        (ik_seed_state[i] > (joint_max_vector_[i] + LIMIT_TOLERANCE))))
-    {
-      ROS_DEBUG_STREAM_NAMED("ikseed", "Not in limits! " << (int)i << " value " << ik_seed_state[i]
-                                                         << " has limit: " << joint_has_limits_vector_[i] << "  being  "
-                                                         << joint_min_vector_[i] << " to " << joint_max_vector_[i]);
-      return false;
-    }
-  }
-
-  std::vector<double> vfree(free_params_.size());
-  for (std::size_t i = 0; i < free_params_.size(); ++i)
-  {
-    int p = free_params_[i];
-    ROS_ERROR("%u is %f", p, ik_seed_state[p]);  // DTC
-    vfree[i] = ik_seed_state[p];
-  }
-
-  KDL::Frame frame;
-  tf::poseMsgToKDL(ik_pose, frame);
-
-  IkSolutionList<IkReal> solutions;
-  int numsol = solve(frame, vfree, solutions);
-  ROS_DEBUG_STREAM_NAMED(name_, "Found " << numsol << " solutions from IKFast");
-
-  std::vector<LimitObeyingSol> solutions_obey_limits;
-
-  if (numsol)
-  {
-    std::vector<double> solution_obey_limits;
-    for (std::size_t s = 0; s < numsol; ++s)
-    {
-      std::vector<double> sol;
-      getSolution(solutions, ik_seed_state, s, sol);
-      ROS_DEBUG_NAMED(name_, "Sol %d: %e   %e   %e   %e   %e   %e", (int)s, sol[0], sol[1], sol[2], sol[3], sol[4],
-                      sol[5]);
-
-      bool obeys_limits = true;
-      for (std::size_t i = 0; i < sol.size(); i++)
-      {
-        // Add tolerance to limit check
-        if (joint_has_limits_vector_[i] && ((sol[i] < (joint_min_vector_[i] - LIMIT_TOLERANCE)) ||
-                                            (sol[i] > (joint_max_vector_[i] + LIMIT_TOLERANCE))))
-        {
-          // One element of solution is not within limits
-          obeys_limits = false;
-          ROS_DEBUG_STREAM_NAMED(name_, "Not in limits! " << (int)i << " value " << sol[i] << " has limit: "
-                                                          << joint_has_limits_vector_[i] << "  being  "
-                                                          << joint_min_vector_[i] << " to " << joint_max_vector_[i]);
-          break;
-        }
-      }
-      if (obeys_limits)
-      {
-        // All elements of this solution obey limits
-        getSolution(solutions, ik_seed_state, s, solution_obey_limits);
-        double dist_from_seed = 0.0;
-        for (std::size_t i = 0; i < ik_seed_state.size(); ++i)
-        {
-          dist_from_seed += fabs(ik_seed_state[i] - solution_obey_limits[i]);
-        }
-
-        solutions_obey_limits.push_back({ solution_obey_limits, dist_from_seed });
-      }
-    }
-  }
-  else
-  {
-    ROS_DEBUG_STREAM_NAMED(name_, "No IK solution");
-  }
-
-  // Sort the solutions under limits and find the one that is closest to ik_seed_state
-  if (!solutions_obey_limits.empty())
-  {
-    std::sort(solutions_obey_limits.begin(), solutions_obey_limits.end());
-    solution = solutions_obey_limits[0].value;
-    error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
-    return true;
-  }
-
-  error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
-  return false;
-}
-
-bool IKFastKinematicsPlugin::getPositionIK(const std::vector<geometry_msgs::Pose>& ik_poses,
-                                           const std::vector<double>& ik_seed_state,
-                                           std::vector<std::vector<double>>& solutions,
-                                           kinematics::KinematicsResult& result,
-                                           const kinematics::KinematicsQueryOptions& options) const
-{
-  ROS_DEBUG_STREAM_NAMED(name_, "getPositionIK with multiple solutions");
-
-  if (!active_)
-  {
-    ROS_ERROR("kinematics not active");
-    result.kinematic_error = kinematics::KinematicErrors::SOLVER_NOT_ACTIVE;
-    return false;
-  }
-
-  if (ik_poses.empty())
-  {
-    ROS_ERROR("ik_poses is empty");
-    result.kinematic_error = kinematics::KinematicErrors::EMPTY_TIP_POSES;
-    return false;
-  }
-
-  if (ik_poses.size() > 1)
-  {
-    ROS_ERROR("ik_poses contains multiple entries, only one is allowed");
-    result.kinematic_error = kinematics::KinematicErrors::MULTIPLE_TIPS_NOT_SUPPORTED;
-    return false;
-  }
-
-  if (ik_seed_state.size() < num_joints_)
-  {
-    ROS_ERROR_STREAM("ik_seed_state only has " << ik_seed_state.size() << " entries, this ikfast solver requires "
-                                               << num_joints_);
-    return false;
-  }
-
-  KDL::Frame frame;
-  tf::poseMsgToKDL(ik_poses[0], frame);
-
-  // solving ik
-  std::vector<IkSolutionList<IkReal>> solution_set;
-  IkSolutionList<IkReal> ik_solutions;
-  std::vector<double> vfree;
-  int numsol = 0;
-  std::vector<double> sampled_joint_vals;
-  if (!redundant_joint_indices_.empty())
-  {
-    // initializing from seed
-    sampled_joint_vals.push_back(ik_seed_state[redundant_joint_indices_[0]]);
-
-    // checking joint limits when using no discretization
-    if (options.discretization_method == kinematics::DiscretizationMethods::NO_DISCRETIZATION &&
-        joint_has_limits_vector_[redundant_joint_indices_.front()])
-    {
-      double joint_min = joint_min_vector_[redundant_joint_indices_.front()];
-      double joint_max = joint_max_vector_[redundant_joint_indices_.front()];
-
-      double jv = sampled_joint_vals[0];
-      if (!((jv > (joint_min - LIMIT_TOLERANCE)) && (jv < (joint_max + LIMIT_TOLERANCE))))
-      {
-        result.kinematic_error = kinematics::KinematicErrors::IK_SEED_OUTSIDE_LIMITS;
-        ROS_ERROR_STREAM("ik seed is out of bounds");
-        return false;
-      }
-    }
-
-    // computing all solutions sets for each sampled value of the redundant joint
-    if (!sampleRedundantJoint(options.discretization_method, sampled_joint_vals))
-    {
-      result.kinematic_error = kinematics::KinematicErrors::UNSUPORTED_DISCRETIZATION_REQUESTED;
-      return false;
-    }
-
-    for (unsigned int i = 0; i < sampled_joint_vals.size(); i++)
-    {
-      vfree.clear();
-      vfree.push_back(sampled_joint_vals[i]);
-      numsol += solve(frame, vfree, ik_solutions);
-      solution_set.push_back(ik_solutions);
-    }
-  }
-  else
-  {
-    // computing for single solution set
-    numsol = solve(frame, vfree, ik_solutions);
-    solution_set.push_back(ik_solutions);
-  }
-
-  ROS_DEBUG_STREAM_NAMED(name_, "Found " << numsol << " solutions from IKFast");
-  bool solutions_found = false;
-  if (numsol > 0)
-  {
-    /*
-      Iterating through all solution sets and storing those that do not exceed joint limits.
-    */
-    for (unsigned int r = 0; r < solution_set.size(); r++)
-    {
-      ik_solutions = solution_set[r];
-      numsol = ik_solutions.GetNumSolutions();
-      for (int s = 0; s < numsol; ++s)
-      {
-        std::vector<double> sol;
-        getSolution(ik_solutions, ik_seed_state, s, sol);
-
-        bool obeys_limits = true;
-        for (unsigned int i = 0; i < sol.size(); i++)
-        {
-          // Add tolerance to limit check
-          if (joint_has_limits_vector_[i] && ((sol[i] < (joint_min_vector_[i] - LIMIT_TOLERANCE)) ||
-                                              (sol[i] > (joint_max_vector_[i] + LIMIT_TOLERANCE))))
-          {
-            // One element of solution is not within limits
-            obeys_limits = false;
-            ROS_DEBUG_STREAM_NAMED(name_, "Not in limits! " << i << " value " << sol[i] << " has limit: "
-                                                            << joint_has_limits_vector_[i] << "  being  "
-                                                            << joint_min_vector_[i] << " to " << joint_max_vector_[i]);
-            break;
-          }
-        }
-        if (obeys_limits)
-        {
-          // All elements of solution obey limits
-          solutions_found = true;
-          solutions.push_back(sol);
-        }
-      }
-    }
-
-    if (solutions_found)
-    {
-      result.kinematic_error = kinematics::KinematicErrors::OK;
-      return true;
-    }
-  }
-  else
-  {
-    ROS_DEBUG_STREAM_NAMED(name_, "No IK solution");
-  }
-
-  result.kinematic_error = kinematics::KinematicErrors::NO_SOLUTION;
-  return false;
-}
-
-bool IKFastKinematicsPlugin::sampleRedundantJoint(kinematics::DiscretizationMethod method,
-                                                  std::vector<double>& sampled_joint_vals) const
-{
-  double joint_min = -M_PI;
-  double joint_max = M_PI;
-  int index = redundant_joint_indices_.front();
-  double joint_dscrt = redundant_joint_discretization_.at(index);
-
-  if (joint_has_limits_vector_[redundant_joint_indices_.front()])
-  {
-    joint_min = joint_min_vector_[index];
-    joint_max = joint_max_vector_[index];
-  }
-
-  switch (method)
-  {
-    case kinematics::DiscretizationMethods::ALL_DISCRETIZED:
-    {
-      int steps = std::ceil((joint_max - joint_min) / joint_dscrt);
-      for (unsigned int i = 0; i < steps; i++)
-      {
-        sampled_joint_vals.push_back(joint_min + joint_dscrt * i);
-      }
-      sampled_joint_vals.push_back(joint_max);
-    }
-    break;
-    case kinematics::DiscretizationMethods::ALL_RANDOM_SAMPLED:
-    {
-      int steps = std::ceil((joint_max - joint_min) / joint_dscrt);
-      steps = steps > 0 ? steps : 1;
-      double diff = joint_max - joint_min;
-      for (int i = 0; i < steps; i++)
-      {
-        sampled_joint_vals.push_back(((diff * std::rand()) / (static_cast<double>(RAND_MAX))) + joint_min);
-      }
-    }
-
-    break;
-    case kinematics::DiscretizationMethods::NO_DISCRETIZATION:
-
-      break;
-    default:
-      ROS_ERROR_STREAM("Discretization method " << method << " is not supported");
-      return false;
-  }
-
-  return true;
-}
-
-}  // end namespace
-
-// register IKFastKinematicsPlugin as a KinematicsBase implementation
-#include <pluginlib/class_list_macros.h>
-PLUGINLIB_EXPORT_CLASS(ikfast_kinematics_plugin::IKFastKinematicsPlugin, kinematics::KinematicsBase);

src/turtlebot_arm/turtlebot_arm_ikfast_plugin/turtlebot_arm_moveit_ikfast_plugin_description.xml

@@ -1,6 +0,0 @@
-<?xml version='1.0' encoding='UTF-8'?>
-<library path="lib/libturtlebot_arm_arm_moveit_ikfast_plugin">
-  <class name="turtlebot_arm_arm_kinematics/IKFastKinematicsPlugin" type="ikfast_kinematics_plugin::IKFastKinematicsPlugin" base_class_type="kinematics::KinematicsBase">
-    <description>IKFast61 plugin for closed-form kinematics</description>
-  </class>
-</library>

src/turtlebot_arm/turtlebot_arm_kinect_calibration/.cproject

@@ -1,42 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" standalone="no"?>
-<?fileVersion 4.0.0?><cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
-	<storageModule moduleId="org.eclipse.cdt.core.settings">
-		<cconfiguration id="cdt.managedbuild.toolchain.gnu.base.1272455299">
-			<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.base.1272455299" moduleId="org.eclipse.cdt.core.settings" name="Default">
-				<externalSettings/>
-				<extensions>
-					<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
-				</extensions>
-			</storageModule>
-			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
-				<configuration buildProperties="" id="cdt.managedbuild.toolchain.gnu.base.1272455299" name="Default" parent="org.eclipse.cdt.build.core.emptycfg">
-					<folderInfo id="cdt.managedbuild.toolchain.gnu.base.1272455299.745420460" name="/" resourcePath="">
-						<toolChain id="cdt.managedbuild.toolchain.gnu.base.1334872349" name="cdt.managedbuild.toolchain.gnu.base" superClass="cdt.managedbuild.toolchain.gnu.base">
-							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="cdt.managedbuild.target.gnu.platform.base.1095742420" name="Debug Platform" osList="linux,hpux,aix,qnx" superClass="cdt.managedbuild.target.gnu.platform.base"/>
-							<builder id="cdt.managedbuild.target.gnu.builder.base.1954269345" managedBuildOn="false" name="Gnu Make Builder.Default" superClass="cdt.managedbuild.target.gnu.builder.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.archiver.base.1343970913" name="GCC Archiver" superClass="cdt.managedbuild.tool.gnu.archiver.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.cpp.compiler.base.988678858" name="GCC C++ Compiler" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.c.compiler.base.980654700" name="GCC C Compiler" superClass="cdt.managedbuild.tool.gnu.c.compiler.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.c.linker.base.1251528329" name="GCC C Linker" superClass="cdt.managedbuild.tool.gnu.c.linker.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.cpp.linker.base.1129763799" name="GCC C++ Linker" superClass="cdt.managedbuild.tool.gnu.cpp.linker.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.assembler.base.117744748" name="GCC Assembler" superClass="cdt.managedbuild.tool.gnu.assembler.base"/>
-						</toolChain>
-					</folderInfo>
-				</configuration>
-			</storageModule>
-			<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
-		</cconfiguration>
-	</storageModule>
-	<storageModule moduleId="cdtBuildSystem" version="4.0.0">
-		<project id="turtlebot_arm_kinect_calibration.null.874760565" name="turtlebot_arm_kinect_calibration"/>
-	</storageModule>
-	<storageModule moduleId="scannerConfiguration">
-		<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
-	</storageModule>
-	<storageModule moduleId="org.eclipse.cdt.core.LanguageSettingsProviders"/>
-</cproject>

src/turtlebot_arm/turtlebot_arm_kinect_calibration/.project

@@ -1,27 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<projectDescription>
-	<name>turtlebot_arm_kinect_calibration</name>
-	<comment></comment>
-	<projects>
-	</projects>
-	<buildSpec>
-		<buildCommand>
-			<name>org.eclipse.cdt.managedbuilder.core.genmakebuilder</name>
-			<triggers>clean,full,incremental,</triggers>
-			<arguments>
-			</arguments>
-		</buildCommand>
-		<buildCommand>
-			<name>org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder</name>
-			<triggers>full,incremental,</triggers>
-			<arguments>
-			</arguments>
-		</buildCommand>
-	</buildSpec>
-	<natures>
-		<nature>org.eclipse.cdt.core.cnature</nature>
-		<nature>org.eclipse.cdt.core.ccnature</nature>
-		<nature>org.eclipse.cdt.managedbuilder.core.managedBuildNature</nature>
-		<nature>org.eclipse.cdt.managedbuilder.core.ScannerConfigNature</nature>
-	</natures>
-</projectDescription>

src/turtlebot_arm/turtlebot_arm_kinect_calibration/CHANGELOG.rst

@@ -1,17 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package turtlebot_arm_kinect_calibration
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-0.3.3 (2014-09-20)
-------------------
-
-0.3.2 (2014-08-30)
-------------------
-
-0.3.1 (2014-08-22)
-------------------
-* Set separated URLs for website, repository and bugtracker
-
-0.3.0 (2014-08-16)
-------------------
-* First indigo release

src/turtlebot_arm/turtlebot_arm_kinect_calibration/CMakeLists.txt

@@ -1,38 +0,0 @@
-cmake_minimum_required(VERSION 2.8.3)
-project(turtlebot_arm_kinect_calibration)
-
-# setup
-find_package(catkin REQUIRED cmake_modules cv_bridge image_geometry image_transport pcl_ros roscpp tf visualization_msgs)
-find_package(Boost REQUIRED system filesystem)
-find_package(PCL REQUIRED)
-find_package(OpenCV REQUIRED)
-
-link_directories(${catkin_LIBRARY_DIRS} ${Boost_LIBRARY_DIRS} ${OpenCV_LIBRARY_DIRS} ${PCL_LIBRARY_DIRS})
-
-catkin_package(DEPENDS cv_bridge image_geometry image_transport pcl_ros roscpp tf visualization_msgs)
-
-include_directories(include
-                    SYSTEM
-                    ${Boost_INCLUDE_DIRS}
-                    ${catkin_INCLUDE_DIRS}
-                    ${PCL_INCLUDE_DIRS}
-                    ${OpenCV_INCLUDE_DIRS}
-                   )
-
-# node
-add_executable(calibrate_kinect_checkerboard src/calibrate_kinect_checkerboard.cpp
-                                             src/detect_calibration_pattern.cpp)
-target_link_libraries(calibrate_kinect_checkerboard ${Boost_LIBRARIES}
-                                                    ${catkin_LIBRARIES}
-                                                    ${PCL_LIBRARIES}
-                                                    ${OpenCV_LIBRARIES})
-
-# install
-
-install(TARGETS calibrate_kinect_checkerboard
-        DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
-install(DIRECTORY launch
-        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)

src/turtlebot_arm/turtlebot_arm_kinect_calibration/include/turtlebot_arm_kinect_calibration/detect_calibration_pattern.h

@@ -1,84 +0,0 @@
-/*
- * Copyright (c) 2011, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- *     * Redistributions of source code must retain the above copyright
- *       notice, this list of conditions and the following disclaimer.
- *     * Redistributions in binary form must reproduce the above copyright
- *       notice, this list of conditions and the following disclaimer in the
- *       documentation and/or other materials provided with the distribution.
- *     * Neither the name of the Willow Garage, Inc. nor the names of its
- *       contributors may be used to endorse or promote products derived from
- *       this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#ifndef _DETECT_CALIBRATION_PATTERN_
-#define _DETECT_CALIBRATION_PATTERN_
-
-#include <opencv2/core/core.hpp>
-#include <opencv2/calib3d/calib3d.hpp>
-#include <opencv2/calib3d/calib3d.hpp>
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
-
-#include <Eigen/Dense>
-#include <Eigen/Geometry> 
-#include <Eigen/StdVector>
-
-#include <iostream>
-#include <stdexcept>
-
-using namespace std;
-
-enum Pattern
-{
-  CHESSBOARD, CIRCLES_GRID, ASYMMETRIC_CIRCLES_GRID
-};
-
-typedef std::vector<cv::Point3f> object_pts_t;
-typedef std::vector<cv::Point2f> observation_pts_t;
-
-void convertCVtoEigen(cv::Mat& tvec, cv::Mat& R, Eigen::Vector3f& translation, Eigen::Quaternionf& orientation);
-
-class PatternDetector
-{
-public:
-  PatternDetector() { }
-
-  static object_pts_t calcChessboardCorners(cv::Size boardSize, float squareSize, Pattern patternType = CHESSBOARD,
-                                            cv::Point3f offset = cv::Point3f());
-
-  int detectPattern(cv::Mat& image_in, Eigen::Vector3f& translation, Eigen::Quaternionf& orientation,
-                    cv::Mat& image_out);
-
-  void setCameraMatrices(cv::Matx33d K_, cv::Mat D_);
-
-  void setPattern(cv::Size grid_size_, float square_size_, Pattern pattern_type_, cv::Point3f offset_ = cv::Point3f());
-
-public:
-  cv::Matx33d K;
-  cv::Mat D;
-  cv::Mat rvec, tvec, R;
-
-  Pattern pattern_type;
-  cv::Size grid_size;
-  float square_size;
-  object_pts_t ideal_points;
-};
-
-#endif

src/turtlebot_arm/turtlebot_arm_kinect_calibration/launch/calibrate.launch

@@ -1,27 +0,0 @@
-<launch>
-  <arg name="publish_camera_tf" default="true"/>
-
-  <!--  *********** Bringup turtlebot arm *********** -->
-  <include file="$(find turtlebot_arm_bringup)/launch/arm.launch" />
-
-  <!--  ************ Bringup the camera ************* -->
-  <include file="$(find turtlebot_bringup)/launch/3dsensor.launch">
-    <arg name="camera"                          value="camera"/>
-    <arg name="3d_sensor"                       value="$(env TURTLEBOT_3D_SENSOR)"/>   <!-- kinect, asus_xtion_pro -->
-    <arg name="publish_tf"                      value="$(arg publish_camera_tf)"/>
-    <!-- We only need pointclouds to detect the blocks -->
-    <arg name="depth_registration"              value="true"/>
-  </include>
-
-  <!--  ************* Calibration stuff ************* -->
-  <node name="calibrate_kinect_checkerboard" pkg="turtlebot_arm_kinect_calibration" type="calibrate_kinect_checkerboard" output="screen">
-    <param name="gripper_tip_x" value="-0.002" />
-    <param name="gripper_tip_y" value="0.020" />
-    <param name="gripper_tip_z" value="-0.0185" />
-  </node>
-
-  <node name="checkerboard_image_view" pkg="image_view" type="image_view">
-    <remap from="image" to="/calibrate_kinect_checkerboard/calibration_pattern_out" />
-    <param name="autosize" value="true" />
-  </node>
-</launch>

src/turtlebot_arm/turtlebot_arm_kinect_calibration/package.xml

@@ -1,35 +0,0 @@
-<package>
-  <name>turtlebot_arm_kinect_calibration</name>
-  <version>0.3.3</version>
-  <description>
-    turtlebot_arm_kinect_calibration allows calibration of a kinect to a TurtleBot arm,
-    including a kinect on-board and off-board the TurtleBot for more precise manipulation.
-  </description>
-  <author>Michael Ferguson</author>
-  <author>Helen Oleynikova</author>
-  <maintainer email="jsantossimon@gmail.com">Jorge Santos</maintainer>
-  <license>BSD</license>
-
-  <url type="website">http://ros.org/wiki/turtlebot_arm_kinect_calibration</url>
-  <url type="repository">https://github.com/turtlebot/turtlebot_arm</url>
-  <url type="bugtracker">https://github.com/turtlebot/turtlebot_arm/issues</url>
-
-  <buildtool_depend>catkin</buildtool_depend>
-
-  <build_depend>cmake_modules</build_depend>
-  <build_depend>cv_bridge</build_depend>
-  <build_depend>image_geometry</build_depend>
-  <build_depend>image_transport</build_depend>
-  <build_depend>pcl_ros</build_depend>
-  <build_depend>roscpp</build_depend>
-  <build_depend>tf</build_depend>
-  <build_depend>visualization_msgs</build_depend>
-
-  <run_depend>cv_bridge</run_depend>
-  <run_depend>image_geometry</run_depend>
-  <run_depend>image_transport</run_depend>
-  <run_depend>pcl_ros</run_depend>
-  <run_depend>roscpp</run_depend>
-  <run_depend>tf</run_depend>
-  <run_depend>visualization_msgs</run_depend>
-</package>

src/turtlebot_arm/turtlebot_arm_kinect_calibration/src/calibrate_kinect_checkerboard.cpp

@@ -1,427 +0,0 @@
-/*
- * Copyright (c) 2011, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- *     * Redistributions of source code must retain the above copyright
- *       notice, this list of conditions and the following disclaimer.
- *     * Redistributions in binary form must reproduce the above copyright
- *       notice, this list of conditions and the following disclaimer in the
- *       documentation and/or other materials provided with the distribution.
- *     * Neither the name of the Willow Garage, Inc. nor the names of its
- *       contributors may be used to endorse or promote products derived from
- *       this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#include <ros/ros.h>
-#include <image_transport/image_transport.h>
-
-#include <cv_bridge/cv_bridge.h>
-#include <image_geometry/pinhole_camera_model.h>
-#include <tf/transform_listener.h>
-#include <tf/transform_broadcaster.h>
-
-#include <pcl/point_types.h>
-#include <pcl_ros/point_cloud.h>
-#include <pcl_ros/transforms.h>
-#include <pcl/registration/registration.h>
-#include <pcl/registration/transformation_estimation_svd.h>
-
-#include <sensor_msgs/Image.h>
-#include <sensor_msgs/CameraInfo.h>
-#include <geometry_msgs/PointStamped.h>
-
-#include <turtlebot_arm_kinect_calibration/detect_calibration_pattern.h>
-
-using namespace std;
-using namespace Eigen;
-
-tf::Transform tfFromEigen(Eigen::Matrix4f trans)
-{
-  tf::Matrix3x3 btm;
-  btm.setValue(trans(0, 0), trans(0, 1), trans(0, 2),
-               trans(1, 0), trans(1, 1), trans(1, 2),
-               trans(2, 0), trans(2, 1), trans(2, 2));
-  tf::Transform ret;
-  ret.setOrigin(tf::Vector3(trans(0, 3), trans(1, 3), trans(2, 3)));
-  ret.setBasis(btm);
-  return ret;
-}
-
-Eigen::Matrix4f EigenFromTF(tf::Transform trans)
-{
-  Eigen::Matrix4f out;
-  tf::Quaternion quat = trans.getRotation();
-  tf::Vector3 origin = trans.getOrigin();
-
-  Eigen::Quaternionf quat_out(quat.w(), quat.x(), quat.y(), quat.z());
-  Eigen::Vector3f origin_out(origin.x(), origin.y(), origin.z());
-
-  out.topLeftCorner<3, 3>() = quat_out.toRotationMatrix();
-  out.topRightCorner<3, 1>() = origin_out;
-  out(3, 3) = 1;
-
-  return out;
-}
-
-class CalibrateKinectCheckerboard
-{
-  // Nodes and publishers/subscribers
-  ros::NodeHandle nh_;
-  image_transport::ImageTransport it_;
-  image_transport::Publisher pub_;
-  ros::Publisher detector_pub_;
-  ros::Publisher physical_pub_;
-
-  // Image and camera info subscribers;
-  ros::Subscriber image_sub_;
-  ros::Subscriber info_sub_;
-
-  // Structures for interacting with ROS messages
-  cv_bridge::CvImagePtr input_bridge_;
-  cv_bridge::CvImagePtr output_bridge_;
-  tf::TransformListener tf_listener_;
-  tf::TransformBroadcaster tf_broadcaster_;
-  image_geometry::PinholeCameraModel cam_model_;
-
-  // Calibration objects
-  PatternDetector pattern_detector_;
-
-  // The optimized transform
-  Eigen::Transform<float, 3, Eigen::Affine> transform_;
-
-  // Visualization for markers
-  pcl::PointCloud<pcl::PointXYZ> detector_points_;
-  pcl::PointCloud<pcl::PointXYZ> ideal_points_;
-  pcl::PointCloud<pcl::PointXYZ> image_points_;
-  pcl::PointCloud<pcl::PointXYZ> physical_points_;
-
-  // Have we calibrated the camera yet?
-  bool calibrated;
-
-  ros::Timer timer_;
-
-  // Parameters
-  std::string fixed_frame;
-  std::string camera_frame;
-  std::string target_frame;
-  std::string tip_frame;
-  std::string touch_frame;
-
-  int checkerboard_width;
-  int checkerboard_height;
-  double checkerboard_grid;
-
-  // Gripper tip position
-  geometry_msgs::PointStamped gripper_tip;
-
-public:
-  CalibrateKinectCheckerboard() :
-      nh_("~"), it_(nh_), calibrated(false)
-  {
-    // Load parameters from the server.
-    nh_.param<std::string>("fixed_frame", fixed_frame, "/base_link");
-    nh_.param<std::string>("camera_frame", camera_frame, "/camera_link");
-    nh_.param<std::string>("target_frame", target_frame, "/calibration_pattern");
-    nh_.param<std::string>("tip_frame", tip_frame, "/gripper_link");
-
-    nh_.param<int>("checkerboard_width", checkerboard_width, 6);
-    nh_.param<int>("checkerboard_height", checkerboard_height, 7);
-    nh_.param<double>("checkerboard_grid", checkerboard_grid, 0.027);
-
-    // Set pattern detector sizes
-    pattern_detector_.setPattern(cv::Size(checkerboard_width, checkerboard_height),
-                                 checkerboard_grid, CHESSBOARD);
-
-    transform_.translation().setZero();
-    transform_.matrix().topLeftCorner<3, 3>() = Quaternionf().setIdentity().toRotationMatrix();
-
-    // Create subscriptions
-    info_sub_ = nh_.subscribe("/camera/rgb/camera_info", 1, &CalibrateKinectCheckerboard::infoCallback, this);
-
-    // Also publishers
-    pub_ = it_.advertise("calibration_pattern_out", 1);
-    detector_pub_ = nh_.advertise<pcl::PointCloud<pcl::PointXYZ> >("detector_cloud", 1);
-    physical_pub_ = nh_.advertise<pcl::PointCloud<pcl::PointXYZ> >("physical_points_cloud", 1);
-
-    // Create ideal points
-    ideal_points_.push_back(pcl::PointXYZ(0, 0, 0));
-    ideal_points_.push_back(pcl::PointXYZ((checkerboard_width - 1) * checkerboard_grid, 0, 0));
-    ideal_points_.push_back(pcl::PointXYZ(0, (checkerboard_height - 1) * checkerboard_grid, 0));
-    ideal_points_.push_back(pcl::PointXYZ((checkerboard_width - 1) * checkerboard_grid,
-                                          (checkerboard_height - 1) * checkerboard_grid, 0));
-
-    // Create proper gripper tip point
-    nh_.param<double>("gripper_tip_x", gripper_tip.point.x, 0.0);
-    nh_.param<double>("gripper_tip_y", gripper_tip.point.y, 0.0);
-    nh_.param<double>("gripper_tip_z", gripper_tip.point.z, 0.0);
-    gripper_tip.header.frame_id = tip_frame;
-
-    ROS_INFO("[calibrate] Initialized.");
-  }
-
-  void infoCallback(const sensor_msgs::CameraInfoConstPtr& info_msg)
-  {
-    if (calibrated)
-      return;
-    cam_model_.fromCameraInfo(info_msg);
-    pattern_detector_.setCameraMatrices(cam_model_.intrinsicMatrix(), cam_model_.distortionCoeffs());
-
-    calibrated = true;
-    image_sub_ = nh_.subscribe("/camera/rgb/image_raw", 1, &CalibrateKinectCheckerboard::imageCallback, this);
-
-    ROS_INFO("[calibrate] Got image info!");
-  }
-
-  void pointcloudCallback(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& msg)
-  {
-    sensor_msgs::ImagePtr image_msg(new sensor_msgs::Image);
-    sensor_msgs::PointCloud2 cloud;
-    pcl::toROSMsg(*msg, cloud);
-    pcl::toROSMsg(cloud, *image_msg);
-
-    imageCallback(image_msg);
-  }
-
-  void imageCallback(const sensor_msgs::ImageConstPtr& image_msg)
-  {
-    try
-    {
-      input_bridge_ = cv_bridge::toCvCopy(image_msg, "mono8");
-      output_bridge_ = cv_bridge::toCvCopy(image_msg, "bgr8");
-    }
-    catch (cv_bridge::Exception& ex)
-    {
-      ROS_ERROR("[calibrate] Failed to convert image:\n%s", ex.what());
-      return;
-    }
-
-    Eigen::Vector3f translation;
-    Eigen::Quaternionf orientation;
-
-    if (!pattern_detector_.detectPattern(input_bridge_->image, translation, orientation, output_bridge_->image))
-    {
-      ROS_INFO("[calibrate] Couldn't detect checkerboard, make sure it's visible in the image.");
-      return;
-    }
-
-    tf::Transform target_transform;
-    tf::StampedTransform base_transform;
-    try
-    {
-      ros::Time acquisition_time = image_msg->header.stamp;
-      ros::Duration timeout(1.0 / 30.0);
-
-      target_transform.setOrigin(tf::Vector3(translation.x(), translation.y(), translation.z()));
-      target_transform.setRotation(tf::Quaternion(orientation.x(), orientation.y(), orientation.z(), orientation.w()));
-      tf_broadcaster_.sendTransform(
-          tf::StampedTransform(target_transform, image_msg->header.stamp, image_msg->header.frame_id, target_frame));
-    }
-    catch (tf::TransformException& ex)
-    {
-      ROS_WARN("[calibrate] TF exception:\n%s", ex.what());
-      return;
-    }
-    publishCloud(ideal_points_, target_transform, image_msg->header.frame_id);
-
-    overlayPoints(ideal_points_, target_transform, output_bridge_);
-
-    // Publish calibration image
-    pub_.publish(output_bridge_->toImageMsg());
-
-    pcl_ros::transformPointCloud(ideal_points_, image_points_, target_transform);
-
-    cout << "Got an image callback!" << endl;
-
-    calibrate(image_msg->header.frame_id);
-
-    ros::shutdown();
-  }
-
-  void publishCloud(pcl::PointCloud<pcl::PointXYZ> detector_points, tf::Transform &transform,
-                    const std::string frame_id)
-  {
-    // Display to rviz
-    pcl::PointCloud < pcl::PointXYZ > transformed_detector_points;
-
-    pcl_ros::transformPointCloud(detector_points, transformed_detector_points, transform);
-
-    transformed_detector_points.header.frame_id = frame_id;
-    detector_pub_.publish(transformed_detector_points);
-  }
-
-  void overlayPoints(pcl::PointCloud<pcl::PointXYZ> detector_points, tf::Transform &transform,
-                     cv_bridge::CvImagePtr& image)
-  {
-    // Overlay calibration points on the image
-    pcl::PointCloud < pcl::PointXYZ > transformed_detector_points;
-
-    pcl_ros::transformPointCloud(detector_points, transformed_detector_points, transform);
-
-    int font_face = cv::FONT_HERSHEY_SCRIPT_SIMPLEX;
-    double font_scale = 1;
-    int thickness = 2;
-    int radius = 5;
-
-    for (unsigned int i = 0; i < transformed_detector_points.size(); i++)
-    {
-      pcl::PointXYZ pt = transformed_detector_points[i];
-      cv::Point3d pt_cv(pt.x, pt.y, pt.z);
-      cv::Point2d uv;
-      uv = cam_model_.project3dToPixel(pt_cv);
-
-      cv::circle(image->image, uv, radius, CV_RGB(255, 0, 0), -1);
-      cv::Size text_size;
-      int baseline = 0;
-      std::stringstream out;
-      out << i + 1;
-
-      text_size = cv::getTextSize(out.str(), font_face, font_scale, thickness, &baseline);
-
-      cv::Point origin = cvPoint(uv.x - text_size.width / 2, uv.y - radius - baseline/* - thickness*/);
-      cv::putText(image->image, out.str(), origin, font_face, font_scale, CV_RGB(255, 0, 0), thickness);
-    }
-  }
-
-  bool calibrate(const std::string frame_id)
-  {
-    physical_points_.empty();
-    physical_points_.header.frame_id = fixed_frame;
-    cout << "Is the checkerboard correct? " << endl;
-    cout << "Move edge of gripper to point 1 in image and press Enter. " << endl;
-    cin.ignore();
-    addPhysicalPoint();
-    cout << "Move edge of gripper to point 2 in image and press Enter. " << endl;
-    cin.ignore();
-    addPhysicalPoint();
-    cout << "Move edge of gripper to point 3 in image and press Enter. " << endl;
-    cin.ignore();
-    addPhysicalPoint();
-    cout << "Move edge of gripper to point 4 in image and press Enter. " << endl;
-    cin.ignore();
-    addPhysicalPoint();
-
-    Eigen::Matrix4f t;
-
-    physical_pub_.publish(physical_points_);
-
-    pcl::registration::TransformationEstimationSVD < pcl::PointXYZ, pcl::PointXYZ > svd_estimator;
-    svd_estimator.estimateRigidTransformation(physical_points_, image_points_, t);
-
-    // Output       
-    tf::Transform transform = tfFromEigen(t), trans_full, camera_transform_unstamped;
-    tf::StampedTransform camera_transform;
-
-    cout << "Resulting transform (camera frame -> fixed frame): " << endl << t << endl << endl;
-
-    try
-    {
-      tf_listener_.lookupTransform(frame_id, camera_frame, ros::Time(0), camera_transform);
-    }
-    catch (tf::TransformException& ex)
-    {
-      ROS_WARN("[calibrate] TF exception:\n%s", ex.what());
-      return false;
-    }
-
-    camera_transform_unstamped = camera_transform;
-    trans_full = camera_transform_unstamped.inverse() * transform;
-
-    Eigen::Matrix4f t_full = EigenFromTF(trans_full);
-    Eigen::Matrix4f t_full_inv = (Eigen::Transform<float, 3, Affine>(t_full).inverse()).matrix();
-
-    cout << "Resulting transform (fixed frame -> camera frame): " << endl << t_full << endl << endl;
-    printStaticTransform(t_full_inv, fixed_frame, camera_frame);
-
-    return true;
-  }
-
-  void printStaticTransform(Eigen::Matrix4f& transform, const std::string frame1, const std::string frame2)
-  {
-    Eigen::Quaternionf quat(transform.topLeftCorner<3, 3>());
-    Eigen::Vector3f translation(transform.topRightCorner<3, 1>());
-
-    cout << "Static transform publisher (use for external kinect): " << endl;
-
-    cout << "rosrun tf static_transform_publisher x y z qx qy qz qw frame_id child_frame_id period_in_ms" << endl;
-    cout << "rosrun tf static_transform_publisher " << translation.x() << " " << translation.y() << " "
-         << translation.z() << " " << quat.x() << " " << quat.y() << " " << quat.z() << " " << quat.w()
-         << " " << frame1 << " " << frame2 << " 100" << endl << endl;
-
-    tf::Transform temp_tf_trans = tfFromEigen(transform);
-
-    double yaw, pitch, roll;
-
-    std::string fixed_frame_urdf(fixed_frame);
-
-    // If there's a leading '/' character, remove it, as xacro can't deal with 
-    // extra characters in the link name.
-    if (fixed_frame_urdf.size() > 0 && fixed_frame_urdf[0] == '/')
-      fixed_frame_urdf.erase(0, 1);
-
-    temp_tf_trans.getBasis().getEulerYPR(yaw, pitch, roll);
-
-    cout << "URDF output (use for kinect on robot): " << endl;
-
-    cout << "<?xml version=\"1.0\"?>\n<robot>\n" << "\t<property name=\"turtlebot_calib_cam_x\" value=\""
-         << translation.x() << "\" />\n" << "\t<property name=\"turtlebot_calib_cam_y\" value=\"" << translation.y()
-         << "\" />\n" << "\t<property name=\"turtlebot_calib_cam_z\" value=\"" << translation.z() << "\" />\n"
-         << "\t<property name=\"turtlebot_calib_cam_rr\" value=\"" << roll << "\" />\n"
-         << "\t<property name=\"turtlebot_calib_cam_rp\" value=\"" << pitch << "\" />\n"
-         << "\t<property name=\"turtlebot_calib_cam_ry\" value=\"" << yaw << "\" />\n"
-         << "\t<property name=\"turtlebot_kinect_frame_name\" value=\"" << fixed_frame_urdf << "\" />\n" << "</robot>"
-         << endl << endl;
-  }
-
-  void addPhysicalPoint()
-  {
-    geometry_msgs::PointStamped pt_out;
-
-    try
-    {
-      tf_listener_.transformPoint(fixed_frame, gripper_tip, pt_out);
-    }
-    catch (tf::TransformException& ex)
-    {
-      ROS_WARN("[calibrate] TF exception:\n%s", ex.what());
-      return;
-    }
-
-    physical_points_.push_back(pcl::PointXYZ(pt_out.point.x, pt_out.point.y, pt_out.point.z));
-  }
-
-  void convertIdealPointstoPointcloud()
-  {
-    detector_points_.points.resize(pattern_detector_.ideal_points.size());
-    for (unsigned int i = 0; i < pattern_detector_.ideal_points.size(); i++)
-    {
-      cv::Point3f pt = pattern_detector_.ideal_points[i];
-      detector_points_[i].x = pt.x;
-      detector_points_[i].y = pt.y;
-      detector_points_[i].z = pt.z;
-    }
-  }
-
-};
-
-int main(int argc, char** argv)
-{
-  ros::init(argc, argv, "calibrate_kinect_arm");
-
-  CalibrateKinectCheckerboard cal;
-  ros::spin();
-}

src/turtlebot_arm/turtlebot_arm_kinect_calibration/src/detect_calibration_pattern.cpp

@@ -1,125 +0,0 @@
-/*
- * Copyright (c) 2011, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- *     * Redistributions of source code must retain the above copyright
- *       notice, this list of conditions and the following disclaimer.
- *     * Redistributions in binary form must reproduce the above copyright
- *       notice, this list of conditions and the following disclaimer in the
- *       documentation and/or other materials provided with the distribution.
- *     * Neither the name of the Willow Garage, Inc. nor the names of its
- *       contributors may be used to endorse or promote products derived from
- *       this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#include <turtlebot_arm_kinect_calibration/detect_calibration_pattern.h>
-
-void PatternDetector::setCameraMatrices(cv::Matx33d K_, cv::Mat D_)
-{
-  K = K_;
-  D = D_;
-}
-
-void PatternDetector::setPattern(cv::Size grid_size_, float square_size_, Pattern pattern_type_, cv::Point3f offset_)
-{
-  ideal_points = calcChessboardCorners(grid_size_, square_size_, pattern_type_, offset_);
-  pattern_type = pattern_type_;
-  grid_size = grid_size_;
-  square_size = square_size_;
-}
-
-object_pts_t PatternDetector::calcChessboardCorners(cv::Size boardSize, float squareSize, Pattern patternType,
-                                                    cv::Point3f offset)
-{
-  object_pts_t corners;
-  switch (patternType)
-  {
-    case CHESSBOARD:
-    case CIRCLES_GRID:
-      for (int i = 0; i < boardSize.height; i++)
-        for (int j = 0; j < boardSize.width; j++)
-          corners.push_back(cv::Point3f(float(j * squareSize), float(i * squareSize), 0) + offset);
-      break;
-    case ASYMMETRIC_CIRCLES_GRID:
-      for (int i = 0; i < boardSize.height; i++)
-        for (int j = 0; j < boardSize.width; j++)
-          corners.push_back(cv::Point3f(float(i * squareSize), float((2 * j + i % 2) * squareSize), 0) + offset);
-      break;
-    default:
-      std::logic_error("Unknown pattern type.");
-  }
-  return corners;
-}
-
-int PatternDetector::detectPattern(cv::Mat& image_in, Eigen::Vector3f& translation, Eigen::Quaternionf& orientation,
-                                   cv::Mat& image_out)
-{
-  translation.setZero();
-  orientation.setIdentity();
-
-  bool found = false;
-
-  observation_pts_t observation_points;
-
-  switch (pattern_type)
-  {
-    case ASYMMETRIC_CIRCLES_GRID:
-      found = cv::findCirclesGrid(image_in, grid_size, observation_points,
-                                  cv::CALIB_CB_ASYMMETRIC_GRID | cv::CALIB_CB_CLUSTERING);
-      break;
-    case CHESSBOARD:
-      found = cv::findChessboardCorners(image_in, grid_size, observation_points, cv::CALIB_CB_ADAPTIVE_THRESH);
-      break;
-    case CIRCLES_GRID:
-      found = cv::findCirclesGrid(image_in, grid_size, observation_points, cv::CALIB_CB_SYMMETRIC_GRID);
-      break;
-  }
-
-  if (found)
-  {
-    // Do subpixel ONLY IF THE PATTERN IS A CHESSBOARD
-    if (pattern_type == CHESSBOARD)
-    {
-      cv::cornerSubPix(image_in, observation_points, cv::Size(5, 5), cv::Size(-1, -1),
-                       cv::TermCriteria(cv::TermCriteria::MAX_ITER + cv::TermCriteria::EPS, 100, 0.01));
-    }
-
-    cv::solvePnP(cv::Mat(ideal_points), cv::Mat(observation_points), K, D, rvec, tvec, false);
-    cv::Rodrigues(rvec, R); //take the 3x1 rotation representation to a 3x3 rotation matrix.
-
-    cv::drawChessboardCorners(image_out, grid_size, cv::Mat(observation_points), found);
-
-    convertCVtoEigen(tvec, R, translation, orientation);
-  }
-
-  return found;
-}
-
-void convertCVtoEigen(cv::Mat& tvec, cv::Mat& R, Eigen::Vector3f& translation, Eigen::Quaternionf& orientation)
-{
-  // This assumes that cv::Mats are stored as doubles. Is there a way to check this?
-  // Since it's templated...
-  translation = Eigen::Vector3f(tvec.at<double>(0, 0), tvec.at<double>(0, 1), tvec.at<double>(0, 2));
-
-  Eigen::Matrix3f Rmat;
-  Rmat << R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2),
-          R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2),
-          R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2);
-
-  orientation = Eigen::Quaternionf(Rmat);
-}

src/turtlebot_arm/turtlebot_arm_moveit_config/.cproject

@@ -1,42 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" standalone="no"?>
-<?fileVersion 4.0.0?><cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
-	<storageModule moduleId="org.eclipse.cdt.core.settings">
-		<cconfiguration id="cdt.managedbuild.toolchain.gnu.base.18311064">
-			<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.base.18311064" moduleId="org.eclipse.cdt.core.settings" name="Default">
-				<externalSettings/>
-				<extensions>
-					<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
-				</extensions>
-			</storageModule>
-			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
-				<configuration buildProperties="" id="cdt.managedbuild.toolchain.gnu.base.18311064" name="Default" parent="org.eclipse.cdt.build.core.emptycfg">
-					<folderInfo id="cdt.managedbuild.toolchain.gnu.base.18311064.1978423688" name="/" resourcePath="">
-						<toolChain id="cdt.managedbuild.toolchain.gnu.base.1757462471" name="Linux GCC" superClass="cdt.managedbuild.toolchain.gnu.base">
-							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="cdt.managedbuild.target.gnu.platform.base.2121900818" name="Debug Platform" osList="linux,hpux,aix,qnx" superClass="cdt.managedbuild.target.gnu.platform.base"/>
-							<builder id="cdt.managedbuild.target.gnu.builder.base.1376723617" managedBuildOn="false" name="Gnu Make Builder.Default" superClass="cdt.managedbuild.target.gnu.builder.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.archiver.base.425643860" name="GCC Archiver" superClass="cdt.managedbuild.tool.gnu.archiver.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.cpp.compiler.base.933285952" name="GCC C++ Compiler" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.c.compiler.base.813276759" name="GCC C Compiler" superClass="cdt.managedbuild.tool.gnu.c.compiler.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.c.linker.base.1440468284" name="GCC C Linker" superClass="cdt.managedbuild.tool.gnu.c.linker.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.cpp.linker.base.216181418" name="GCC C++ Linker" superClass="cdt.managedbuild.tool.gnu.cpp.linker.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.assembler.base.598429478" name="GCC Assembler" superClass="cdt.managedbuild.tool.gnu.assembler.base"/>
-						</toolChain>
-					</folderInfo>
-				</configuration>
-			</storageModule>
-			<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
-		</cconfiguration>
-	</storageModule>
-	<storageModule moduleId="cdtBuildSystem" version="4.0.0">
-		<project id="turtlebot_arm_moveit_config.null.660516957" name="turtlebot_arm_moveit_config"/>
-	</storageModule>
-	<storageModule moduleId="scannerConfiguration">
-		<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
-	</storageModule>
-	<storageModule moduleId="org.eclipse.cdt.core.LanguageSettingsProviders"/>
-</cproject>

src/turtlebot_arm/turtlebot_arm_moveit_config/.project

@@ -1,27 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<projectDescription>
-	<name>turtlebot_arm_moveit_config</name>
-	<comment></comment>
-	<projects>
-	</projects>
-	<buildSpec>
-		<buildCommand>
-			<name>org.eclipse.cdt.managedbuilder.core.genmakebuilder</name>
-			<triggers>clean,full,incremental,</triggers>
-			<arguments>
-			</arguments>
-		</buildCommand>
-		<buildCommand>
-			<name>org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder</name>
-			<triggers>full,incremental,</triggers>
-			<arguments>
-			</arguments>
-		</buildCommand>
-	</buildSpec>
-	<natures>
-		<nature>org.eclipse.cdt.core.cnature</nature>
-		<nature>org.eclipse.cdt.core.ccnature</nature>
-		<nature>org.eclipse.cdt.managedbuilder.core.managedBuildNature</nature>
-		<nature>org.eclipse.cdt.managedbuilder.core.ScannerConfigNature</nature>
-	</natures>
-</projectDescription>

src/turtlebot_arm/turtlebot_arm_moveit_config/.setup_assistant

@@ -1,10 +0,0 @@
-moveit_setup_assistant_config:
-  URDF:
-    package: turtlebot_arm_description
-    relative_path: urdf/turtlebot_arm.urdf
-  SRDF:
-    relative_path: config/turtlebot_arm.srdf
-  CONFIG:
-    author_name: Jorge Santos
-    author_email: jsantossimon@gmail.com
-    generated_timestamp: 1488412003

src/turtlebot_arm/turtlebot_arm_moveit_config/CHANGELOG.rst

@@ -1,18 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package turtlebot_arm_moveit_config
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-0.3.3 (2014-09-20)
-------------------
-
-0.3.2 (2014-08-30)
-------------------
-* Add turtlebot_arm_moveit.launch: a version of demo.launch with simulation on/off
-
-0.3.1 (2014-08-22)
-------------------
-* Set separated URLs for website, repository and bugtracker
-
-0.3.0 (2014-08-16)
-------------------
-* First indigo release

src/turtlebot_arm/turtlebot_arm_moveit_config/CMakeLists.txt

@@ -1,10 +0,0 @@
-cmake_minimum_required(VERSION 2.8.3)
-project(turtlebot_arm_moveit_config)
-
-find_package(catkin REQUIRED)
-
-catkin_package()
-
-install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-  PATTERN "setup_assistant.launch" EXCLUDE)
-install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

src/turtlebot_arm/turtlebot_arm_moveit_config/config/controllers.yaml

@@ -1,18 +0,0 @@
-controller_list:
-  - name: arm_controller
-    action_ns: follow_joint_trajectory
-    type: FollowJointTrajectory
-    default: true
-    joints:
-      - arm_shoulder_pan_joint
-      - arm_shoulder_lift_joint
-      - arm_elbow_flex_joint
-      - arm_wrist_flex_joint
-      - gripper_link_joint
-  - name: gripper_controller
-    action_ns: gripper_action
-    type: GripperCommand
-    default: true
-    command_joint: gripper_joint
-    joints:
-      - gripper_joint

src/turtlebot_arm/turtlebot_arm_moveit_config/config/fake_controllers.yaml

@@ -1,11 +0,0 @@
-controller_list:
-  - name: fake_arm_controller
-    joints:
-      - arm_shoulder_pan_joint
-      - arm_shoulder_lift_joint
-      - arm_elbow_flex_joint
-      - arm_wrist_flex_joint
-      - gripper_link_joint
-  - name: fake_gripper_controller
-    joints:
-      - gripper_joint

src/turtlebot_arm/turtlebot_arm_moveit_config/config/joint_limits.yaml

@@ -1,34 +0,0 @@
-# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
-# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
-# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
-joint_limits:
-  arm_elbow_flex_joint:
-    has_velocity_limits: true
-    max_velocity: 1.571
-    has_acceleration_limits: false
-    max_acceleration: 0
-  arm_shoulder_lift_joint:
-    has_velocity_limits: true
-    max_velocity: 1.571
-    has_acceleration_limits: false
-    max_acceleration: 0
-  arm_shoulder_pan_joint:
-    has_velocity_limits: true
-    max_velocity: 1.571
-    has_acceleration_limits: false
-    max_acceleration: 0
-  arm_wrist_flex_joint:
-    has_velocity_limits: true
-    max_velocity: 1.571
-    has_acceleration_limits: false
-    max_acceleration: 0
-  gripper_joint:
-    has_velocity_limits: true
-    max_velocity: 0.785
-    has_acceleration_limits: false
-    max_acceleration: 0
-  gripper_link_joint:
-    has_velocity_limits: true
-    max_velocity: 1
-    has_acceleration_limits: false
-    max_acceleration: 0

src/turtlebot_arm/turtlebot_arm_moveit_config/config/kinematics.yaml

@@ -1,5 +0,0 @@
-arm:
-  kinematics_solver: turtlebot_arm_arm_kinematics/IKFastKinematicsPlugin
-  kinematics_solver_search_resolution: 0.005
-  kinematics_solver_timeout: 0.005
-  kinematics_solver_attempts: 3

src/turtlebot_arm/turtlebot_arm_moveit_config/config/ompl_planning.yaml

@@ -1,172 +0,0 @@
-planner_configs:
-  SBLkConfigDefault:
-    type: geometric::SBL
-    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
-  ESTkConfigDefault:
-    type: geometric::EST
-    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup()
-    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
-  LBKPIECEkConfigDefault:
-    type: geometric::LBKPIECE
-    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
-    border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9
-    min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
-  BKPIECEkConfigDefault:
-    type: geometric::BKPIECE
-    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
-    border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9
-    failed_expansion_score_factor: 0.5  # When extending motion fails, scale score by factor. default: 0.5
-    min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
-  KPIECEkConfigDefault:
-    type: geometric::KPIECE
-    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
-    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05 
-    border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9 (0.0,1.]
-    failed_expansion_score_factor: 0.5  # When extending motion fails, scale score by factor. default: 0.5
-    min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
-  RRTkConfigDefault:
-    type: geometric::RRT
-    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
-    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
-  RRTConnectkConfigDefault:
-    type: geometric::RRTConnect
-    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
-  RRTstarkConfigDefault:
-    type: geometric::RRTstar
-    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
-    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
-    delay_collision_checking: 1  # Stop collision checking as soon as C-free parent found. default 1
-  TRRTkConfigDefault:
-    type: geometric::TRRT
-    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
-    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
-    max_states_failed: 10  # when to start increasing temp. default: 10
-    temp_change_factor: 2.0  # how much to increase or decrease temp. default: 2.0
-    min_temperature: 10e-10  # lower limit of temp change. default: 10e-10
-    init_temperature: 10e-6  # initial temperature. default: 10e-6
-    frountier_threshold: 0.0  # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() 
-    frountierNodeRatio: 0.1  # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
-    k_constant: 0.0  # value used to normalize expresssion. default: 0.0 set in setup()
-  PRMkConfigDefault:
-    type: geometric::PRM
-    max_nearest_neighbors: 10  # use k nearest neighbors. default: 10
-  PRMstarkConfigDefault:
-    type: geometric::PRMstar
-  FMTkConfigDefault:
-    type: geometric::FMT
-    num_samples: 1000  # number of states that the planner should sample. default: 1000
-    radius_multiplier: 1.1  # multiplier used for the nearest neighbors search radius. default: 1.1
-    nearest_k: 1  # use Knearest strategy. default: 1
-    cache_cc: 1  # use collision checking cache. default: 1
-    heuristics: 0  # activate cost to go heuristics. default: 0
-    extended_fmt: 1  # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1
-  BFMTkConfigDefault:
-    type: geometric::BFMT
-    num_samples: 1000  # number of states that the planner should sample. default: 1000
-    radius_multiplier: 1.0  # multiplier used for the nearest neighbors search radius. default: 1.0
-    nearest_k: 1  # use the Knearest strategy. default: 1
-    balanced: 0  # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1
-    optimality: 1  # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1
-    heuristics: 1  # activates cost to go heuristics. default: 1
-    cache_cc: 1  # use the collision checking cache. default: 1
-    extended_fmt: 1  # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1
-  PDSTkConfigDefault:
-    type: geometric::PDST
-  STRIDEkConfigDefault:
-    type: geometric::STRIDE
-    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
-    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05 
-    use_projected_distance: 0  # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0
-    degree: 16  # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 
-    max_degree: 18  # max degree of a node in the GNAT. default: 12
-    min_degree: 12  # min degree of a node in the GNAT. default: 12
-    max_pts_per_leaf: 6  # max points per leaf in the GNAT. default: 6
-    estimated_dimension: 0.0  # estimated dimension of the free space. default: 0.0
-    min_valid_path_fraction: 0.2  # Accept partially valid moves above fraction. default: 0.2
-  BiTRRTkConfigDefault:
-    type: geometric::BiTRRT
-    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
-    temp_change_factor: 0.1  # how much to increase or decrease temp. default: 0.1
-    init_temperature: 100  # initial temperature. default: 100
-    frountier_threshold: 0.0  # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() 
-    frountier_node_ratio: 0.1  # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
-    cost_threshold: 1e300  # the cost threshold. Any motion cost that is not better will not be expanded. default: inf
-  LBTRRTkConfigDefault:
-    type: geometric::LBTRRT
-    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
-    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05 
-    epsilon: 0.4  # optimality approximation factor. default: 0.4
-  BiESTkConfigDefault:
-    type: geometric::BiEST
-    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
-  ProjESTkConfigDefault:
-    type: geometric::ProjEST
-    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
-    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05 
-  LazyPRMkConfigDefault:
-    type: geometric::LazyPRM
-    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
-  LazyPRMstarkConfigDefault:
-    type: geometric::LazyPRMstar
-  SPARSkConfigDefault:
-    type: geometric::SPARS
-    stretch_factor: 3.0  # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
-    sparse_delta_fraction: 0.25  # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
-    dense_delta_fraction: 0.001  # delta fraction for interface detection. default: 0.001
-    max_failures: 1000  # maximum consecutive failure limit. default: 1000
-  SPARStwokConfigDefault:
-    type: geometric::SPARStwo
-    stretch_factor: 3.0  # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
-    sparse_delta_fraction: 0.25  # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
-    dense_delta_fraction: 0.001  # delta fraction for interface detection. default: 0.001
-    max_failures: 5000  # maximum consecutive failure limit. default: 5000
-arm:
-  planner_configs:
-    - SBLkConfigDefault
-    - ESTkConfigDefault
-    - LBKPIECEkConfigDefault
-    - BKPIECEkConfigDefault
-    - KPIECEkConfigDefault
-    - RRTkConfigDefault
-    - RRTConnectkConfigDefault
-    - RRTstarkConfigDefault
-    - TRRTkConfigDefault
-    - PRMkConfigDefault
-    - PRMstarkConfigDefault
-    - FMTkConfigDefault
-    - BFMTkConfigDefault
-    - PDSTkConfigDefault
-    - STRIDEkConfigDefault
-    - BiTRRTkConfigDefault
-    - LBTRRTkConfigDefault
-    - BiESTkConfigDefault
-    - ProjESTkConfigDefault
-    - LazyPRMkConfigDefault
-    - LazyPRMstarkConfigDefault
-    - SPARSkConfigDefault
-    - SPARStwokConfigDefault
-gripper:
-  planner_configs:
-    - SBLkConfigDefault
-    - ESTkConfigDefault
-    - LBKPIECEkConfigDefault
-    - BKPIECEkConfigDefault
-    - KPIECEkConfigDefault
-    - RRTkConfigDefault
-    - RRTConnectkConfigDefault
-    - RRTstarkConfigDefault
-    - TRRTkConfigDefault
-    - PRMkConfigDefault
-    - PRMstarkConfigDefault
-    - FMTkConfigDefault
-    - BFMTkConfigDefault
-    - PDSTkConfigDefault
-    - STRIDEkConfigDefault
-    - BiTRRTkConfigDefault
-    - LBTRRTkConfigDefault
-    - BiESTkConfigDefault
-    - ProjESTkConfigDefault
-    - LazyPRMkConfigDefault
-    - LazyPRMstarkConfigDefault
-    - SPARSkConfigDefault
-    - SPARStwokConfigDefault

src/turtlebot_arm/turtlebot_arm_moveit_config/config/pincher_arm.srdf

@@ -1,261 +0,0 @@
-<?xml version="1.0" ?>
-<!--This does not replace URDF, and is not an extension of URDF.
-    This is a format for representing semantic information about the robot structure.
-    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
--->
-<robot name="turtlebot_arm">
-    <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
-    <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
-    <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
-    <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
-    <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
-    <group name="arm">
-        <chain base_link="base_link" tip_link="gripper_link" />
-    </group>
-    <group name="gripper">
-        <joint name="gripper_servo_joint" />
-        <joint name="gripper2_joint" />
-        <joint name="gripper_finger_base_joint" />
-        <joint name="gripper_joint" />
-    </group>
-    <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
-    <group_state name="right_up" group="arm">
-        <joint name="arm_elbow_flex_joint" value="0" />
-        <joint name="arm_shoulder_lift_joint" value="0" />
-        <joint name="arm_shoulder_pan_joint" value="0" />
-        <joint name="arm_wrist_flex_joint" value="0" />
-        <joint name="gripper_link_joint" value="0" />
-    </group_state>
-    <group_state name="resting" group="arm">
-        <joint name="arm_elbow_flex_joint" value="0.01" />
-        <joint name="arm_shoulder_lift_joint" value="0" />
-        <joint name="arm_shoulder_pan_joint" value="0" />
-        <joint name="arm_wrist_flex_joint" value="-0.01" />
-        <joint name="gripper_link_joint" value="0" />
-    </group_state>
-    <group_state name="forward" group="arm">
-        <joint name="arm_elbow_flex_joint" value="0.2646" />
-        <joint name="arm_shoulder_lift_joint" value="0.294" />
-        <joint name="arm_shoulder_pan_joint" value="0" />
-        <joint name="arm_wrist_flex_joint" value="0.2549" />
-        <joint name="gripper_link_joint" value="0" />
-    </group_state>
-    <group_state name="forward_left" group="arm">
-        <joint name="arm_elbow_flex_joint" value="0.2646" />
-        <joint name="arm_shoulder_lift_joint" value="0.294" />
-        <joint name="arm_shoulder_pan_joint" value="1.382" />
-        <joint name="arm_wrist_flex_joint" value="0.2549" />
-        <joint name="gripper_link_joint" value="0" />
-    </group_state>
-    <group_state name="forward_right" group="arm">
-        <joint name="arm_elbow_flex_joint" value="0.2646" />
-        <joint name="arm_shoulder_lift_joint" value="0.294" />
-        <joint name="arm_shoulder_pan_joint" value="-1.6761" />
-        <joint name="arm_wrist_flex_joint" value="0.2549" />
-        <joint name="gripper_link_joint" value="0" />
-    </group_state>
-    <group_state name="down" group="arm">
-        <joint name="arm_elbow_flex_joint" value="1.2056" />
-        <joint name="arm_shoulder_lift_joint" value="0.7057" />
-        <joint name="arm_shoulder_pan_joint" value="0" />
-        <joint name="arm_wrist_flex_joint" value="1.1372" />
-        <joint name="gripper_link_joint" value="0" />
-    </group_state>
-    <group_state name="down_left" group="arm">
-        <joint name="arm_elbow_flex_joint" value="1.2056" />
-        <joint name="arm_shoulder_lift_joint" value="0.7057" />
-        <joint name="arm_shoulder_pan_joint" value="1.3232" />
-        <joint name="arm_wrist_flex_joint" value="1.1372" />
-        <joint name="gripper_link_joint" value="0" />
-    </group_state>
-    <group_state name="down_right" group="arm">
-        <joint name="arm_elbow_flex_joint" value="1.2056" />
-        <joint name="arm_shoulder_lift_joint" value="0.7057" />
-        <joint name="arm_shoulder_pan_joint" value="-1.7055" />
-        <joint name="arm_wrist_flex_joint" value="1.1372" />
-        <joint name="gripper_link_joint" value="0" />
-    </group_state>
-    <group_state name="grip_open" group="gripper">
-        <joint name="gripper_joint" value="0.031" />
-    </group_state>
-    <group_state name="grip_closed" group="gripper">
-        <joint name="gripper_joint" value="0.002" />
-    </group_state>
-    <group_state name="grip_mid" group="gripper">
-        <joint name="gripper_joint" value="0.015" />
-    </group_state>
-    <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
-    <end_effector name="gripper" parent_link="gripper_link" group="gripper" parent_group="arm" />
-    <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
-    <virtual_joint name="world_joint" type="fixed" parent_frame="reference_frame" child_link="base_link" />
-    <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
-    <disable_collisions link1="arm_elbow_F10_0_link" link2="arm_elbow_F10_1_link" reason="Adjacent" />
-    <disable_collisions link1="arm_elbow_F10_0_link" link2="arm_elbow_F10_2_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_0_link" link2="arm_elbow_F3_0_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_0_link" link2="arm_elbow_flex_link" reason="Adjacent" />
-    <disable_collisions link1="arm_elbow_F10_0_link" link2="arm_elbow_flex_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_0_link" link2="arm_shoulder_F10_0_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_0_link" link2="arm_shoulder_F10_1_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_0_link" link2="arm_shoulder_F10_2_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_0_link" link2="arm_shoulder_F3_0_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_0_link" link2="arm_shoulder_lift_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_0_link" link2="arm_shoulder_lift_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_0_link" link2="arm_shoulder_pan_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_0_link" link2="arm_wrist_F3_0_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_0_link" link2="arm_wrist_flex_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_0_link" link2="arm_wrist_flex_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_0_link" link2="gripper_active2_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_0_link" link2="gripper_active_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_0_link" link2="gripper_finger_base_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_0_link" link2="gripper_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_1_link" link2="arm_elbow_F10_2_link" reason="Adjacent" />
-    <disable_collisions link1="arm_elbow_F10_1_link" link2="arm_elbow_F3_0_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_1_link" link2="arm_elbow_flex_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_1_link" link2="arm_elbow_flex_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_1_link" link2="arm_shoulder_F10_0_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_1_link" link2="arm_shoulder_F10_1_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_1_link" link2="arm_shoulder_F10_2_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_1_link" link2="arm_shoulder_F3_0_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_1_link" link2="arm_shoulder_lift_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_1_link" link2="arm_shoulder_lift_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_1_link" link2="arm_shoulder_pan_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_1_link" link2="arm_wrist_F3_0_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_1_link" link2="arm_wrist_flex_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_1_link" link2="arm_wrist_flex_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_1_link" link2="gripper_active2_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_1_link" link2="gripper_active_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_1_link" link2="gripper_finger_base_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_1_link" link2="gripper_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_2_link" link2="arm_elbow_F3_0_link" reason="Adjacent" />
-    <disable_collisions link1="arm_elbow_F10_2_link" link2="arm_elbow_flex_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_2_link" link2="arm_elbow_flex_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_2_link" link2="arm_shoulder_F10_0_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_2_link" link2="arm_shoulder_F10_1_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_2_link" link2="arm_shoulder_F10_2_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_2_link" link2="arm_shoulder_F3_0_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_2_link" link2="arm_shoulder_lift_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_2_link" link2="arm_shoulder_lift_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_2_link" link2="arm_shoulder_pan_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_2_link" link2="arm_wrist_F3_0_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_2_link" link2="arm_wrist_flex_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_2_link" link2="arm_wrist_flex_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_2_link" link2="gripper_active2_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_2_link" link2="gripper_active_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_2_link" link2="gripper_finger_base_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_2_link" link2="gripper_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F3_0_link" link2="arm_elbow_flex_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F3_0_link" link2="arm_elbow_flex_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F3_0_link" link2="arm_shoulder_F10_0_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F3_0_link" link2="arm_shoulder_F10_1_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F3_0_link" link2="arm_shoulder_F10_2_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F3_0_link" link2="arm_shoulder_F3_0_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F3_0_link" link2="arm_shoulder_lift_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F3_0_link" link2="arm_shoulder_lift_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F3_0_link" link2="arm_shoulder_pan_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F3_0_link" link2="arm_wrist_F3_0_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F3_0_link" link2="arm_wrist_flex_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F3_0_link" link2="arm_wrist_flex_servo_link" reason="Adjacent" />
-    <disable_collisions link1="arm_elbow_F3_0_link" link2="gripper_active2_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F3_0_link" link2="gripper_active_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F3_0_link" link2="gripper_finger_base_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F3_0_link" link2="gripper_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_link" link2="arm_elbow_flex_servo_link" reason="Adjacent" />
-    <disable_collisions link1="arm_elbow_flex_link" link2="arm_shoulder_lift_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_link" link2="arm_shoulder_pan_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_link" link2="arm_wrist_F3_0_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_link" link2="arm_wrist_flex_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_link" link2="arm_wrist_flex_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_link" link2="gripper_active2_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_link" link2="gripper_active_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_link" link2="gripper_finger_base_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_link" link2="gripper_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_servo_link" link2="arm_shoulder_F10_0_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_servo_link" link2="arm_shoulder_F10_1_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_servo_link" link2="arm_shoulder_F10_2_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_servo_link" link2="arm_shoulder_F3_0_link" reason="Adjacent" />
-    <disable_collisions link1="arm_elbow_flex_servo_link" link2="arm_shoulder_lift_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_servo_link" link2="arm_shoulder_lift_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_servo_link" link2="arm_shoulder_pan_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_servo_link" link2="arm_wrist_F3_0_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_servo_link" link2="arm_wrist_flex_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_servo_link" link2="arm_wrist_flex_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_servo_link" link2="gripper_active2_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_servo_link" link2="gripper_active_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_servo_link" link2="gripper_finger_base_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_servo_link" link2="gripper_servo_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_0_link" link2="arm_shoulder_F10_1_link" reason="Adjacent" />
-    <disable_collisions link1="arm_shoulder_F10_0_link" link2="arm_shoulder_F10_2_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_0_link" link2="arm_shoulder_F3_0_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_0_link" link2="arm_shoulder_lift_link" reason="Adjacent" />
-    <disable_collisions link1="arm_shoulder_F10_0_link" link2="arm_shoulder_lift_servo_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_0_link" link2="arm_shoulder_pan_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_0_link" link2="arm_wrist_F3_0_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_0_link" link2="arm_wrist_flex_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_0_link" link2="arm_wrist_flex_servo_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_0_link" link2="gripper_active2_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_0_link" link2="gripper_active_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_0_link" link2="gripper_finger_base_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_0_link" link2="gripper_servo_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_1_link" link2="arm_shoulder_F10_2_link" reason="Adjacent" />
-    <disable_collisions link1="arm_shoulder_F10_1_link" link2="arm_shoulder_F3_0_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_1_link" link2="arm_shoulder_lift_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_1_link" link2="arm_shoulder_lift_servo_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_1_link" link2="arm_shoulder_pan_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_1_link" link2="arm_wrist_F3_0_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_1_link" link2="arm_wrist_flex_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_1_link" link2="arm_wrist_flex_servo_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_1_link" link2="gripper_active2_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_1_link" link2="gripper_active_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_1_link" link2="gripper_finger_base_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_1_link" link2="gripper_servo_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_2_link" link2="arm_shoulder_F3_0_link" reason="Adjacent" />
-    <disable_collisions link1="arm_shoulder_F10_2_link" link2="arm_shoulder_lift_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_2_link" link2="arm_shoulder_lift_servo_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_2_link" link2="arm_shoulder_pan_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_2_link" link2="arm_wrist_F3_0_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_2_link" link2="arm_wrist_flex_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_2_link" link2="arm_wrist_flex_servo_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_2_link" link2="gripper_active2_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_2_link" link2="gripper_active_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_2_link" link2="gripper_finger_base_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_2_link" link2="gripper_servo_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F3_0_link" link2="arm_shoulder_lift_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F3_0_link" link2="arm_shoulder_lift_servo_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F3_0_link" link2="arm_shoulder_pan_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F3_0_link" link2="arm_wrist_F3_0_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F3_0_link" link2="arm_wrist_flex_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F3_0_link" link2="arm_wrist_flex_servo_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F3_0_link" link2="gripper_active2_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F3_0_link" link2="gripper_active_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F3_0_link" link2="gripper_finger_base_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F3_0_link" link2="gripper_servo_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_lift_link" link2="arm_shoulder_lift_servo_link" reason="Adjacent" />
-    <disable_collisions link1="arm_shoulder_lift_link" link2="arm_wrist_F3_0_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_lift_link" link2="arm_wrist_flex_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_lift_link" link2="arm_wrist_flex_servo_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_lift_servo_link" link2="arm_shoulder_pan_link" reason="Adjacent" />
-    <disable_collisions link1="arm_shoulder_lift_servo_link" link2="arm_shoulder_pan_servo_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_lift_servo_link" link2="arm_wrist_flex_servo_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_pan_link" link2="arm_shoulder_pan_servo_link" reason="Adjacent" />
-    <disable_collisions link1="arm_wrist_F3_0_link" link2="arm_wrist_flex_link" reason="Adjacent" />
-    <disable_collisions link1="arm_wrist_F3_0_link" link2="arm_wrist_flex_servo_link" reason="Never" />
-    <disable_collisions link1="arm_wrist_F3_0_link" link2="gripper_active2_link" reason="Never" />
-    <disable_collisions link1="arm_wrist_F3_0_link" link2="gripper_active_link" reason="Never" />
-    <disable_collisions link1="arm_wrist_F3_0_link" link2="gripper_finger_base_link" reason="Never" />
-    <disable_collisions link1="arm_wrist_F3_0_link" link2="gripper_servo_link" reason="Adjacent" />
-    <disable_collisions link1="arm_wrist_flex_link" link2="arm_wrist_flex_servo_link" reason="Adjacent" />
-    <disable_collisions link1="arm_wrist_flex_link" link2="gripper_active2_link" reason="Never" />
-    <disable_collisions link1="arm_wrist_flex_link" link2="gripper_active_link" reason="Never" />
-    <disable_collisions link1="arm_wrist_flex_link" link2="gripper_finger_base_link" reason="Never" />
-    <disable_collisions link1="arm_wrist_flex_link" link2="gripper_servo_link" reason="Default" />
-    <disable_collisions link1="arm_wrist_flex_servo_link" link2="gripper_active2_link" reason="Never" />
-    <disable_collisions link1="arm_wrist_flex_servo_link" link2="gripper_active_link" reason="Never" />
-    <disable_collisions link1="arm_wrist_flex_servo_link" link2="gripper_finger_base_link" reason="Never" />
-    <disable_collisions link1="arm_wrist_flex_servo_link" link2="gripper_servo_link" reason="Never" />
-    <disable_collisions link1="gripper_active2_link" link2="gripper_active_link" reason="Default" />
-    <disable_collisions link1="gripper_active2_link" link2="gripper_finger_base_link" reason="Default" />
-    <disable_collisions link1="gripper_active2_link" link2="gripper_servo_link" reason="Adjacent" />
-    <disable_collisions link1="gripper_active_link" link2="gripper_finger_base_link" reason="Default" />
-    <disable_collisions link1="gripper_active_link" link2="gripper_servo_link" reason="Adjacent" />
-    <disable_collisions link1="gripper_finger_base_link" link2="gripper_servo_link" reason="Adjacent" />
-</robot>

src/turtlebot_arm/turtlebot_arm_moveit_config/config/turtlebot_arm.srdf

@@ -1,228 +0,0 @@
-<?xml version="1.0" ?>
-<!--This does not replace URDF, and is not an extension of URDF.
-    This is a format for representing semantic information about the robot structure.
-    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
--->
-<robot name="turtlebot_arm">
-    <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
-    <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
-    <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
-    <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
-    <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
-    <group name="arm">
-        <chain base_link="arm_base_link" tip_link="gripper_link" />
-    </group>
-    <group name="gripper">
-        <link name="gripper_servo_link" />
-        <link name="gripper_active_finger_link" />
-        <link name="gripper_active_link" />
-        <link name="gripper_static_finger_link" />
-        <link name="gripper_static_link" />
-        <joint name="gripper_joint" />
-    </group>
-    <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
-    <group_state name="right_up" group="arm">
-        <joint name="arm_elbow_flex_joint" value="0" />
-        <joint name="arm_shoulder_lift_joint" value="0" />
-        <joint name="arm_shoulder_pan_joint" value="0" />
-        <joint name="arm_wrist_flex_joint" value="0" />
-    </group_state>
-
-    <group_state name="resting" group="arm">
-        <joint name="arm_elbow_flex_joint" value="2.3" />
-        <joint name="arm_shoulder_lift_joint" value="-2" />
-        <joint name="arm_shoulder_pan_joint" value="0" />
-        <joint name="arm_wrist_flex_joint" value="1.4" />
-    </group_state>
-    <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
-    <end_effector name="gripper" parent_link="gripper_link" group="gripper" parent_group="arm" />
-    <!--PASSIVE JOINT: Purpose: this element is used to mark joints that are not actuated-->
-    <passive_joint name="gripper_link_joint" />
-    <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
-    <disable_collisions link1="arm_elbow_F10_0_link" link2="arm_elbow_F10_1_link" reason="Adjacent" />
-    <disable_collisions link1="arm_elbow_F10_0_link" link2="arm_elbow_F10_2_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_0_link" link2="arm_elbow_F3_0_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_0_link" link2="arm_elbow_flex_link" reason="Adjacent" />
-    <disable_collisions link1="arm_elbow_F10_0_link" link2="arm_elbow_flex_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_0_link" link2="arm_shoulder_F10_0_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_0_link" link2="arm_shoulder_F10_1_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_0_link" link2="arm_shoulder_F10_2_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_0_link" link2="arm_shoulder_F3_0_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_0_link" link2="arm_shoulder_lift_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_0_link" link2="arm_shoulder_lift_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_0_link" link2="arm_shoulder_pan_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_0_link" link2="arm_wrist_F3_0_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_0_link" link2="arm_wrist_flex_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_0_link" link2="arm_wrist_flex_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_0_link" link2="gripper_active_finger_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_0_link" link2="gripper_active_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_0_link" link2="gripper_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_0_link" link2="gripper_static_finger_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_0_link" link2="gripper_static_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_1_link" link2="arm_elbow_F10_2_link" reason="Adjacent" />
-    <disable_collisions link1="arm_elbow_F10_1_link" link2="arm_elbow_F3_0_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_1_link" link2="arm_elbow_flex_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_1_link" link2="arm_elbow_flex_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_1_link" link2="arm_shoulder_F10_0_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_1_link" link2="arm_shoulder_F10_1_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_1_link" link2="arm_shoulder_F10_2_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_1_link" link2="arm_shoulder_F3_0_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_1_link" link2="arm_shoulder_lift_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_1_link" link2="arm_shoulder_lift_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_1_link" link2="arm_shoulder_pan_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_1_link" link2="arm_wrist_F3_0_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_1_link" link2="arm_wrist_flex_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_1_link" link2="arm_wrist_flex_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_1_link" link2="gripper_active_finger_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_1_link" link2="gripper_active_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_1_link" link2="gripper_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_1_link" link2="gripper_static_finger_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_1_link" link2="gripper_static_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_2_link" link2="arm_elbow_F3_0_link" reason="Adjacent" />
-    <disable_collisions link1="arm_elbow_F10_2_link" link2="arm_elbow_flex_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_2_link" link2="arm_elbow_flex_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_2_link" link2="arm_shoulder_F10_0_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_2_link" link2="arm_shoulder_F10_1_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_2_link" link2="arm_shoulder_F10_2_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_2_link" link2="arm_shoulder_F3_0_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_2_link" link2="arm_shoulder_lift_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_2_link" link2="arm_shoulder_lift_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_2_link" link2="arm_shoulder_pan_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_2_link" link2="arm_wrist_F3_0_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_2_link" link2="arm_wrist_flex_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_2_link" link2="arm_wrist_flex_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_2_link" link2="gripper_active_finger_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_2_link" link2="gripper_active_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_2_link" link2="gripper_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_2_link" link2="gripper_static_finger_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F10_2_link" link2="gripper_static_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F3_0_link" link2="arm_elbow_flex_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F3_0_link" link2="arm_elbow_flex_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F3_0_link" link2="arm_shoulder_F10_0_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F3_0_link" link2="arm_shoulder_F10_1_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F3_0_link" link2="arm_shoulder_F10_2_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F3_0_link" link2="arm_shoulder_F3_0_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F3_0_link" link2="arm_shoulder_lift_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F3_0_link" link2="arm_shoulder_lift_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F3_0_link" link2="arm_shoulder_pan_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F3_0_link" link2="arm_wrist_F3_0_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F3_0_link" link2="arm_wrist_flex_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F3_0_link" link2="arm_wrist_flex_servo_link" reason="Adjacent" />
-    <disable_collisions link1="arm_elbow_F3_0_link" link2="gripper_active_finger_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F3_0_link" link2="gripper_active_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F3_0_link" link2="gripper_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F3_0_link" link2="gripper_static_finger_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_F3_0_link" link2="gripper_static_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_link" link2="arm_elbow_flex_servo_link" reason="Adjacent" />
-    <disable_collisions link1="arm_elbow_flex_link" link2="arm_shoulder_lift_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_link" link2="arm_shoulder_pan_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_link" link2="arm_wrist_F3_0_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_link" link2="arm_wrist_flex_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_link" link2="arm_wrist_flex_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_link" link2="gripper_active_finger_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_link" link2="gripper_active_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_link" link2="gripper_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_link" link2="gripper_static_finger_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_link" link2="gripper_static_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_servo_link" link2="arm_shoulder_F10_0_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_servo_link" link2="arm_shoulder_F10_1_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_servo_link" link2="arm_shoulder_F10_2_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_servo_link" link2="arm_shoulder_F3_0_link" reason="Adjacent" />
-    <disable_collisions link1="arm_elbow_flex_servo_link" link2="arm_shoulder_lift_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_servo_link" link2="arm_shoulder_lift_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_servo_link" link2="arm_shoulder_pan_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_servo_link" link2="arm_wrist_F3_0_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_servo_link" link2="arm_wrist_flex_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_servo_link" link2="arm_wrist_flex_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_servo_link" link2="gripper_active_finger_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_servo_link" link2="gripper_active_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_servo_link" link2="gripper_servo_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_servo_link" link2="gripper_static_finger_link" reason="Never" />
-    <disable_collisions link1="arm_elbow_flex_servo_link" link2="gripper_static_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_0_link" link2="arm_shoulder_F10_1_link" reason="Adjacent" />
-    <disable_collisions link1="arm_shoulder_F10_0_link" link2="arm_shoulder_F10_2_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_0_link" link2="arm_shoulder_F3_0_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_0_link" link2="arm_shoulder_lift_link" reason="Adjacent" />
-    <disable_collisions link1="arm_shoulder_F10_0_link" link2="arm_shoulder_lift_servo_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_0_link" link2="arm_shoulder_pan_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_0_link" link2="arm_wrist_F3_0_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_0_link" link2="arm_wrist_flex_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_0_link" link2="arm_wrist_flex_servo_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_0_link" link2="gripper_active_finger_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_0_link" link2="gripper_active_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_0_link" link2="gripper_servo_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_0_link" link2="gripper_static_finger_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_0_link" link2="gripper_static_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_1_link" link2="arm_shoulder_F10_2_link" reason="Adjacent" />
-    <disable_collisions link1="arm_shoulder_F10_1_link" link2="arm_shoulder_F3_0_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_1_link" link2="arm_shoulder_lift_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_1_link" link2="arm_shoulder_lift_servo_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_1_link" link2="arm_shoulder_pan_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_1_link" link2="arm_wrist_F3_0_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_1_link" link2="arm_wrist_flex_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_1_link" link2="arm_wrist_flex_servo_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_1_link" link2="gripper_active_finger_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_1_link" link2="gripper_active_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_1_link" link2="gripper_servo_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_1_link" link2="gripper_static_finger_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_1_link" link2="gripper_static_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_2_link" link2="arm_shoulder_F3_0_link" reason="Adjacent" />
-    <disable_collisions link1="arm_shoulder_F10_2_link" link2="arm_shoulder_lift_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_2_link" link2="arm_shoulder_lift_servo_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_2_link" link2="arm_shoulder_pan_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_2_link" link2="arm_wrist_F3_0_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_2_link" link2="arm_wrist_flex_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_2_link" link2="arm_wrist_flex_servo_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_2_link" link2="gripper_active_finger_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_2_link" link2="gripper_active_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_2_link" link2="gripper_servo_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_2_link" link2="gripper_static_finger_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F10_2_link" link2="gripper_static_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F3_0_link" link2="arm_shoulder_lift_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F3_0_link" link2="arm_shoulder_lift_servo_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F3_0_link" link2="arm_shoulder_pan_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F3_0_link" link2="arm_wrist_F3_0_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F3_0_link" link2="arm_wrist_flex_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F3_0_link" link2="arm_wrist_flex_servo_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F3_0_link" link2="gripper_active_finger_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F3_0_link" link2="gripper_active_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F3_0_link" link2="gripper_servo_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F3_0_link" link2="gripper_static_finger_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_F3_0_link" link2="gripper_static_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_lift_link" link2="arm_shoulder_lift_servo_link" reason="Adjacent" />
-    <disable_collisions link1="arm_shoulder_lift_link" link2="arm_wrist_flex_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_lift_link" link2="arm_wrist_flex_servo_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_lift_link" link2="gripper_static_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_lift_servo_link" link2="arm_shoulder_pan_link" reason="Adjacent" />
-    <disable_collisions link1="arm_shoulder_lift_servo_link" link2="arm_shoulder_pan_servo_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_lift_servo_link" link2="arm_wrist_flex_servo_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_lift_servo_link" link2="gripper_static_link" reason="Never" />
-    <disable_collisions link1="arm_shoulder_pan_link" link2="arm_shoulder_pan_servo_link" reason="Adjacent" />
-    <disable_collisions link1="arm_shoulder_pan_link" link2="gripper_static_link" reason="Never" />
-    <disable_collisions link1="arm_wrist_F3_0_link" link2="arm_wrist_flex_link" reason="Adjacent" />
-    <disable_collisions link1="arm_wrist_F3_0_link" link2="arm_wrist_flex_servo_link" reason="Never" />
-    <disable_collisions link1="arm_wrist_F3_0_link" link2="gripper_active_finger_link" reason="Never" />
-    <disable_collisions link1="arm_wrist_F3_0_link" link2="gripper_active_link" reason="Default" />
-    <disable_collisions link1="arm_wrist_F3_0_link" link2="gripper_servo_link" reason="Adjacent" />
-    <disable_collisions link1="arm_wrist_F3_0_link" link2="gripper_static_finger_link" reason="Default" />
-    <disable_collisions link1="arm_wrist_F3_0_link" link2="gripper_static_link" reason="Default" />
-    <disable_collisions link1="arm_wrist_flex_link" link2="arm_wrist_flex_servo_link" reason="Adjacent" />
-    <disable_collisions link1="arm_wrist_flex_link" link2="gripper_servo_link" reason="Never" />
-    <disable_collisions link1="arm_wrist_flex_link" link2="gripper_static_finger_link" reason="Never" />
-    <disable_collisions link1="arm_wrist_flex_link" link2="gripper_static_link" reason="Never" />
-    <disable_collisions link1="arm_wrist_flex_servo_link" link2="gripper_active_finger_link" reason="Never" />
-    <disable_collisions link1="arm_wrist_flex_servo_link" link2="gripper_active_link" reason="Never" />
-    <disable_collisions link1="arm_wrist_flex_servo_link" link2="gripper_servo_link" reason="Never" />
-    <disable_collisions link1="arm_wrist_flex_servo_link" link2="gripper_static_finger_link" reason="Never" />
-    <disable_collisions link1="arm_wrist_flex_servo_link" link2="gripper_static_link" reason="Never" />
-    <disable_collisions link1="gripper_active_finger_link" link2="gripper_active_link" reason="Adjacent" />
-    <disable_collisions link1="gripper_active_finger_link" link2="gripper_servo_link" reason="Default" />
-    <disable_collisions link1="gripper_active_finger_link" link2="gripper_static_finger_link" reason="Never" />
-    <disable_collisions link1="gripper_active_finger_link" link2="gripper_static_link" reason="Never" />
-    <disable_collisions link1="gripper_active_link" link2="gripper_servo_link" reason="Adjacent" />
-    <disable_collisions link1="gripper_active_link" link2="gripper_static_finger_link" reason="Never" />
-    <disable_collisions link1="gripper_active_link" link2="gripper_static_link" reason="Never" />
-    <disable_collisions link1="gripper_servo_link" link2="gripper_static_finger_link" reason="Default" />
-    <disable_collisions link1="gripper_servo_link" link2="gripper_static_link" reason="Adjacent" />
-    <disable_collisions link1="gripper_static_finger_link" link2="gripper_static_link" reason="Adjacent" />
-</robot>

src/turtlebot_arm/turtlebot_arm_moveit_config/launch/default_warehouse_db.launch

@@ -1,15 +0,0 @@
-<launch>
-
-  <arg name="reset" default="false"/>
-  <!-- If not specified, we'll use a default database location -->
-  <arg name="moveit_warehouse_database_path" default="$(find turtlebot_arm_moveit_config)/default_warehouse_mongo_db" />
-
-  <!-- Launch the warehouse with the configured database location -->
-  <include file="$(find turtlebot_arm_moveit_config)/launch/warehouse.launch">
-    <arg name="moveit_warehouse_database_path" value="$(arg moveit_warehouse_database_path)" />
-  </include>
-
-  <!-- If we want to reset the database, run this node -->
-  <node if="$(arg reset)" name="$(anon moveit_default_db_reset)" type="moveit_init_demo_warehouse" pkg="moveit_ros_warehouse" respawn="false" output="screen" />
-
-</launch>

src/turtlebot_arm/turtlebot_arm_moveit_config/launch/demo.launch

@@ -1,43 +0,0 @@
-<launch>
-
-  <!-- By default, we do not start a database (it can be large) -->
-  <arg name="db" default="false" />
-
-  <!-- Allow user to specify database location -->
-  <arg name="db_path" default="$(find turtlebot_arm_moveit_config)/default_warehouse_mongo_db" />
-
-  <!-- By default, we are not in debug mode -->
-  <arg name="debug" default="false" />
-
-  <!-- Load arm description, state and controllers in simulation mode -->
-  <include file="$(find turtlebot_arm_bringup)/launch/arm.launch">
-    <arg name="simulation" value="false"/>
-  </include>
-
-  <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
-  <include file="$(find turtlebot_arm_moveit_config)/launch/planning_context.launch">
-    <arg name="load_robot_description" value="false"/>
-  </include>
-
-  <!-- If needed, broadcast static tf for robot root -->
-
-  <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
-  <include file="$(find turtlebot_arm_moveit_config)/launch/move_group.launch">
-    <arg name="allow_trajectory_execution" value="true"/>
-    <arg name="fake_execution" value="false"/>
-    <arg name="info" value="true"/>
-    <arg name="debug" value="$(arg debug)"/>
-  </include>
-
-  <!-- Run Rviz and load the default config to see the state of the move_group node -->
-  <include file="$(find turtlebot_arm_moveit_config)/launch/moveit_rviz.launch">
-    <arg name="config" value="true"/>
-    <arg name="debug" value="$(arg debug)"/>
-  </include>
-
-  <!-- If database loading was enabled, start mongodb as well -->
-  <include file="$(find turtlebot_arm_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)">
-    <arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
-  </include>
-
-</launch>

src/turtlebot_arm/turtlebot_arm_moveit_config/launch/fake_moveit_controller_manager.launch.xml

@@ -1,9 +0,0 @@
-<launch>
-
-  <!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
-  <param name="moveit_controller_manager" value="moveit_fake_controller_manager/MoveItFakeControllerManager"/>
-
-  <!-- The rest of the params are specific to this plugin -->
-  <rosparam file="$(find turtlebot_arm_moveit_config)/config/fake_controllers.yaml"/>
-
-</launch>

src/turtlebot_arm/turtlebot_arm_moveit_config/launch/joystick_control.launch

@@ -1,17 +0,0 @@
-<launch>
-  <!-- See moveit_ros/visualization/doc/joystick.rst for documentation -->
-
-  <arg name="dev" default="/dev/input/js0" />
-
-  <!-- Launch joy node -->
-  <node pkg="joy" type="joy_node" name="joy">
-    <param name="dev" value="$(arg dev)" /> <!-- Customize this to match the location your joystick is plugged in on-->
-    <param name="deadzone" value="0.2" />
-    <param name="autorepeat_rate" value="40" />
-    <param name="coalesce_interval" value="0.025" />
-  </node>
-
-  <!-- Launch python interface -->
-  <node pkg="moveit_ros_visualization" type="moveit_joy.py" output="screen" name="moveit_joy"/>
-        
-</launch>

src/turtlebot_arm/turtlebot_arm_moveit_config/launch/move_group.launch

@@ -1,72 +0,0 @@
-<launch>
-
-  <include file="$(find turtlebot_arm_moveit_config)/launch/planning_context.launch" />
-
-  <!-- GDB Debug Option -->
-  <arg name="debug" default="false" />
-  <arg unless="$(arg debug)" name="launch_prefix" value="" />
-  <arg     if="$(arg debug)" name="launch_prefix"
-	   value="gdb -x $(find turtlebot_arm_moveit_config)/launch/gdb_settings.gdb --ex run --args" />
-
-  <!-- Verbose Mode Option -->
-  <arg name="info" default="$(arg debug)" />
-  <arg unless="$(arg info)" name="command_args" value="" />
-  <arg     if="$(arg info)" name="command_args" value="--debug" />
-
-  <!-- move_group settings -->
-  <arg name="allow_trajectory_execution" default="true"/>
-  <arg name="fake_execution" default="false"/>
-  <arg name="max_safe_path_cost" default="1"/>
-  <arg name="jiggle_fraction" default="0.05" />
-  <arg name="publish_monitored_planning_scene" default="true"/>
-
-  <!-- Planning Functionality -->
-  <include ns="move_group" file="$(find turtlebot_arm_moveit_config)/launch/planning_pipeline.launch.xml">
-    <arg name="pipeline" value="ompl" />
-  </include>
-
-  <!-- Trajectory Execution Functionality -->
-  <include ns="move_group" file="$(find turtlebot_arm_moveit_config)/launch/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
-    <arg name="moveit_manage_controllers" value="true" />
-    <arg name="moveit_controller_manager" value="turtlebot_arm" unless="$(arg fake_execution)"/>
-    <arg name="moveit_controller_manager" value="fake" if="$(arg fake_execution)"/>
-  </include>
-
-  <!-- Sensors Functionality -->
-  <include ns="move_group" file="$(find turtlebot_arm_moveit_config)/launch/sensor_manager.launch.xml" if="$(arg allow_trajectory_execution)">
-    <arg name="moveit_sensor_manager" value="turtlebot_arm" />
-  </include>
-
-  <!-- Start the actual move_group node/action server -->
-  <node name="move_group" launch-prefix="$(arg launch_prefix)" pkg="moveit_ros_move_group" type="move_group" respawn="false" output="screen" args="$(arg command_args)">
-    <!-- Set the display variable, in case OpenGL code is used internally -->
-    <env name="DISPLAY" value="$(optenv DISPLAY :0)" />
-
-    <param name="allow_trajectory_execution" value="$(arg allow_trajectory_execution)"/>
-    <param name="max_safe_path_cost" value="$(arg max_safe_path_cost)"/>
-    <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
-
-    <!-- load these non-default MoveGroup capabilities -->
-    <!--
-    <param name="capabilities" value="
-                  a_package/AwsomeMotionPlanningCapability
-                  another_package/GraspPlanningPipeline
-                  " />
-    -->
-
-    <!-- inhibit these default MoveGroup capabilities -->
-    <!--
-    <param name="disable_capabilities" value="
-                  move_group/MoveGroupKinematicsService
-                  move_group/ClearOctomapService
-                  " />
-    -->
-
-    <!-- Publish the planning scene of the physical robot so that rviz plugin can know actual robot -->
-    <param name="planning_scene_monitor/publish_planning_scene" value="$(arg publish_monitored_planning_scene)" />
-    <param name="planning_scene_monitor/publish_geometry_updates" value="$(arg publish_monitored_planning_scene)" />
-    <param name="planning_scene_monitor/publish_state_updates" value="$(arg publish_monitored_planning_scene)" />
-    <param name="planning_scene_monitor/publish_transforms_updates" value="$(arg publish_monitored_planning_scene)" />
-  </node>
-
-</launch>

src/turtlebot_arm/turtlebot_arm_moveit_config/launch/moveit.rviz

@@ -1,689 +0,0 @@
-Panels:
-  - Class: rviz/Displays
-    Help Height: 84
-    Name: Displays
-    Property Tree Widget:
-      Expanded:
-        - /MotionPlanning1
-      Splitter Ratio: 0.74256
-    Tree Height: 664
-  - Class: rviz/Help
-    Name: Help
-  - Class: rviz/Views
-    Expanded:
-      - /Current View1
-    Name: Views
-    Splitter Ratio: 0.5
-Visualization Manager:
-  Class: ""
-  Displays:
-    - Alpha: 0.5
-      Cell Size: 1
-      Class: rviz/Grid
-      Color: 160; 160; 164
-      Enabled: true
-      Line Style:
-        Line Width: 0.03
-        Value: Lines
-      Name: Grid
-      Normal Cell Count: 0
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Plane: XY
-      Plane Cell Count: 10
-      Reference Frame: <Fixed Frame>
-      Value: true
-    - Class: moveit_rviz_plugin/MotionPlanning
-      Enabled: true
-      MoveIt_Goal_Tolerance: 0
-      MoveIt_Planning_Time: 5
-      MoveIt_Use_Constraint_Aware_IK: true
-      MoveIt_Warehouse_Host: 127.0.0.1
-      MoveIt_Warehouse_Port: 33829
-      Name: MotionPlanning
-      Planned Path:
-        Links:
-          base_bellow_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          base_footprint:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          base_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          bl_caster_l_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          bl_caster_r_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          bl_caster_rotation_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          br_caster_l_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          br_caster_r_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          br_caster_rotation_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          double_stereo_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fl_caster_l_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fl_caster_r_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fl_caster_rotation_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fr_caster_l_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fr_caster_r_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fr_caster_rotation_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_mount_kinect_ir_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_mount_kinect_rgb_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_mount_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_mount_prosilica_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_pan_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_plate_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_tilt_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_elbow_flex_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_forearm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_forearm_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_l_finger_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_l_finger_tip_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_motor_accelerometer_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_palm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_r_finger_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_r_finger_tip_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_shoulder_lift_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_shoulder_pan_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_upper_arm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_upper_arm_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_wrist_flex_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_wrist_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          laser_tilt_mount_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_elbow_flex_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_forearm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_forearm_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_l_finger_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_l_finger_tip_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_motor_accelerometer_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_palm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_r_finger_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_r_finger_tip_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_shoulder_lift_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_shoulder_pan_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_upper_arm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_upper_arm_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_wrist_flex_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_wrist_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          sensor_mount_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          torso_lift_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-        Loop Animation: true
-        Robot Alpha: 0.5
-        Show Robot Collision: false
-        Show Robot Visual: true
-        Show Trail: false
-        State Display Time: 0.05 s
-        Trajectory Topic: move_group/display_planned_path
-      Planning Metrics:
-        Payload: 1
-        Show Joint Torques: false
-        Show Manipulability: false
-        Show Manipulability Index: false
-        Show Weight Limit: false
-      Planning Request:
-        Colliding Link Color: 255; 0; 0
-        Goal State Alpha: 1
-        Goal State Color: 250; 128; 0
-        Interactive Marker Size: 0
-        Joint Violation Color: 255; 0; 255
-        Planning Group: left_arm
-        Query Goal State: true
-        Query Start State: false
-        Show Workspace: false
-        Start State Alpha: 1
-        Start State Color: 0; 255; 0
-      Planning Scene Topic: move_group/monitored_planning_scene
-      Robot Description: robot_description
-      Scene Geometry:
-        Scene Alpha: 1
-        Scene Color: 50; 230; 50
-        Scene Display Time: 0.2
-        Show Scene Geometry: true
-        Voxel Coloring: Z-Axis
-        Voxel Rendering: Occupied Voxels
-      Scene Robot:
-        Attached Body Color: 150; 50; 150
-        Links:
-          base_bellow_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          base_footprint:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          base_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          bl_caster_l_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          bl_caster_r_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          bl_caster_rotation_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          br_caster_l_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          br_caster_r_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          br_caster_rotation_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          double_stereo_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fl_caster_l_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fl_caster_r_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fl_caster_rotation_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fr_caster_l_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fr_caster_r_wheel_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          fr_caster_rotation_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_mount_kinect_ir_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_mount_kinect_rgb_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_mount_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_mount_prosilica_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_pan_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_plate_frame:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          head_tilt_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_elbow_flex_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_forearm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_forearm_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_l_finger_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_l_finger_tip_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_motor_accelerometer_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_palm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_r_finger_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_gripper_r_finger_tip_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_shoulder_lift_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_shoulder_pan_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_upper_arm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_upper_arm_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_wrist_flex_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          l_wrist_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          laser_tilt_mount_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_elbow_flex_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_forearm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_forearm_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_l_finger_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_l_finger_tip_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_motor_accelerometer_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_palm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_r_finger_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_gripper_r_finger_tip_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_shoulder_lift_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_shoulder_pan_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_upper_arm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_upper_arm_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_wrist_flex_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          r_wrist_roll_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          sensor_mount_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          torso_lift_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-        Robot Alpha: 0.5
-        Show Scene Robot: true
-      Value: true
-  Enabled: true
-  Global Options:
-    Background Color: 48; 48; 48
-    Fixed Frame: /base_link
-  Name: root
-  Tools:
-    - Class: rviz/Interact
-      Hide Inactive Objects: true
-    - Class: rviz/MoveCamera
-    - Class: rviz/Select
-  Value: true
-  Views:
-    Current:
-      Class: rviz/XYOrbit
-      Distance: 2.9965
-      Focal Point:
-        X: 0.113567
-        Y: 0.10592
-        Z: 2.23518e-07
-      Name: Current View
-      Near Clip Distance: 0.01
-      Pitch: 0.509797
-      Target Frame: /base_link
-      Value: XYOrbit (rviz)
-      Yaw: 5.65995
-    Saved: ~
-Window Geometry:
-  Displays:
-    collapsed: false
-  Height: 1337
-  Help:
-    collapsed: false
-  Hide Left Dock: false
-  Hide Right Dock: false
-  Motion Planning:
-    collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000100000000000002a2000004bcfc0200000005fb000000100044006900730070006c00610079007301000000410000032d000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a0056006900650077007300000003b0000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000017400ffffff0000047a000004bc00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
-  Views:
-    collapsed: false
-  Width: 1828
-  X: 459
-  Y: -243

src/turtlebot_arm/turtlebot_arm_moveit_config/launch/moveit_rviz.launch

@@ -1,16 +0,0 @@
-<launch>
-
-  <arg name="debug" default="false" />
-  <arg unless="$(arg debug)" name="launch_prefix" value="" />
-  <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
-
-  <arg name="config" default="false" />
-  <arg unless="$(arg config)" name="command_args" value="" />
-  <arg     if="$(arg config)" name="command_args" value="-d $(find turtlebot_arm_moveit_config)/launch/moveit.rviz" />
-  
-  <node name="$(anon rviz)" launch-prefix="$(arg launch_prefix)" pkg="rviz" type="rviz" respawn="false"
-	args="$(arg command_args)" output="screen">
-    <rosparam command="load" file="$(find turtlebot_arm_moveit_config)/config/kinematics.yaml"/>
-  </node>
-
-</launch>

src/turtlebot_arm/turtlebot_arm_moveit_config/launch/ompl_planning_pipeline.launch.xml

@@ -1,22 +0,0 @@
-<launch>
-
-  <!-- OMPL Plugin for MoveIt! -->
-  <arg name="planning_plugin" value="ompl_interface/OMPLPlanner" />
-
-  <!-- The request adapters (plugins) used when planning with OMPL. 
-       ORDER MATTERS -->
-  <arg name="planning_adapters" value="default_planner_request_adapters/AddTimeParameterization
-				       default_planner_request_adapters/FixWorkspaceBounds
-				       default_planner_request_adapters/FixStartStateBounds
-				       default_planner_request_adapters/FixStartStateCollision
-				       default_planner_request_adapters/FixStartStatePathConstraints" />
-
-  <arg name="start_state_max_bounds_error" value="0.1" />
-
-  <param name="planning_plugin" value="$(arg planning_plugin)" />
-  <param name="request_adapters" value="$(arg planning_adapters)" />
-  <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
-
-  <rosparam command="load" file="$(find turtlebot_arm_moveit_config)/config/ompl_planning.yaml"/>
-
-</launch>

src/turtlebot_arm/turtlebot_arm_moveit_config/launch/planning_context.launch

@@ -1,27 +0,0 @@
-<launch>
-  <arg name="arm_type" default="$(optenv TURTLEBOT_ARM1 turtlebot)"/>
-
-  <!-- By default we do not overwrite the URDF. Change the following to true to change the default behavior -->
-  <arg name="load_robot_description" default="false"/>
-
-  <!-- The name of the parameter under which the URDF is loaded -->
-  <arg name="robot_description" default="robot_description"/>
-
-  <!-- Load universal robot description format (URDF) -->
-  <param if="$(arg load_robot_description)" name="$(arg robot_description)"
-    command="$(find xacro)/xacro --inorder '$(find turtlebot_arm_description)/urdf/$(arg arm_type)_arm.urdf.xacro'"/>
-
-  <!-- The semantic description that corresponds to the URDF -->
-  <param name="$(arg robot_description)_semantic" textfile="$(find turtlebot_arm_moveit_config)/config/$(arg arm_type)_arm.srdf" />
-
-  <!-- Load updated joint limits (override information from URDF) -->
-  <group ns="$(arg robot_description)_planning">
-    <rosparam command="load" file="$(find turtlebot_arm_moveit_config)/config/joint_limits.yaml"/>
-  </group>
-
-  <!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->
-  <group ns="$(arg robot_description)_kinematics">
-    <rosparam command="load" file="$(find turtlebot_arm_moveit_config)/config/kinematics.yaml"/>
-  </group>
-
-</launch>