Initial commit

warehouse-picking-automation-challenges · Oct 4, 2016 · d6175e2 · d6175e2
commit d6175e2
Show file tree

Hide file tree

Showing 746 changed files with 995,762 additions and 0 deletions.
diff --git a/LICENSE b/LICENSE
@@ -0,0 +1,31 @@
+Copyright (c) 2016, Adam Tow, Andrew English, Chris Lehnert, Chris McCool,
+Fangyi Zhang, Jake Dean, James Sergeant, John Skinner, Juxi Leitner, Lachlan
+Nicholson, Liao Wu, Markus Eich, Matthew Cooper, Niko Suenderhauf, Peter Kujala,
+Ruben Mangels, Steven Martin, Trung Pham, ARC Centre of Excellence for Robotic
+Vision, Queensland University of Technology, The University of Adelaide
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+list of conditions and the following disclaimer.
+
+2. 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.
+
+3. Neither the name of the copyright holder 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 HOLDER 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.
diff --git a/README.md b/README.md
@@ -0,0 +1,137 @@
+# Installation Instructions
+
+## Various Required Packages
+
+```
+sudo apt-get install libeigen3-dev
+sudo apt-get install libnlopt-dev
+sudo apt-get install libxmlrpc-c++8-dev
+sudo apt-get install libudev-dev
+sudo apt-get install ros-indigo-pcl-conversions
+sudo apt-get install ros-indigo-ar-track-alvar
+sudo apt-get install python-sklearn python-termcolor
+```
+
+## Various Not Necessarily Required packages
+
+```
+sudo apt-get install cmake-curses-gui
+```
+
+## ROS Indigo
+
+```
+sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
+sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net --recv-key 0xB01FA116
+sudo apt-get update
+sudo apt-get install ros-indigo-desktop-full
+apt-cache search ros-indigo
+sudo rosdep init
+rosdep update
+sudo apt-get install python-rosinstall
+```
+
+## MoveIt!
+
+```
+sudo apt-get install ros-indigo-moveit-*
+```
+
+## PCL
+
+Place this wherever you checkout non-ROS repos. I use a checkout folder in my home directory, `co` for short.
+
+Note: To get version `1.8.0`, checkout `master` at the `pcl-1.8.0` tag and create the branch `local-1.8.0` so that we do not remain in a detached head state. The commands below include this step.
+
+```
+mkdir ~/co
+cd ~/co
+git clone https://github.com/PointCloudLibrary/pcl.git
+cd pcl
+git checkout tags/pcl-1.8.0 -b local-1.8.0
+cd ~/ros_ws/src/apc_docs/pcl_patch
+./pcl_fix_pre.sh
+cd -
+mkdir build
+cd build
+cmake .. -DCMAKE_BUILD_TYPE=Release -DBUILD_GPU=ON -DBUILD_CUDA=ON
+make -j7
+sudo make -j7 install
+cd ~/ros_ws/src/apc_docs/pcl_patch
+./pcl_fix_post.sh
+```
+
+## Segmentation Library
+
+Build and install segmentation library before building segmentation_ros.
+
+```
+cd ~/ros_ws/src/acrv_apc/segmentation
+mkdir build
+cd build
+cmake ..
+make
+sudo make install
+```
+
+## librealsense
+
+Place this wherever you checkout non-ROS repos. I use a checkout folder in my home directory, `co` for short.
+
+```
+cd ~/co
+git clone https://github.com/IntelRealSense/librealsense.git
+```
+
+Then follow instructions at: https://github.com/IntelRealSense/librealsense/blob/master/doc/installation.md#ubuntu-installation
+
+## Baxter SDK
+
+Create the following directory structure:
+
+```
+|-ros_ws
+|--|-baxter.sh (wget https://github.com/RethinkRobotics/baxter/raw/master/baxter.sh && chmod u+x baxter.sh)
+|--|-src
+|--|--|-acrv_apc (This repository.)
+|--|--|-baxter_sdk (This is where to put baxter sdk packages like baxter_common, baxter_interface, etc.)
+|--|--|-CMakeLists.txt (This is a default CMakeLists file.)
+```
+
+Run the following commands:
+
+```
+sudo apt-get update
+sudo apt-get install git-core python-argparse python-wstool python-vcstools python-rosdep ros-indigo-control-msgs ros-indigo-joystick-drivers
+cd ~/ros_ws/src
+mkdir baxter_sdk
+cd baxter_sdk
+wstool init .
+wstool merge https://raw.githubusercontent.com/RethinkRobotics/baxter/master/baxter_sdk.rosinstall
+wstool update
+source /opt/ros/indigo/setup.bash
+touch baxter_common/baxter_description/CATKIN_IGNORE
+touch baxter_common/rethink_ee_description/CATKIN_IGNORE
+cd ~/ros_ws
+catkin_make
+catkin_make install
+wget https://github.com/RethinkRobotics/baxter/raw/master/baxter.sh
+chmod u+x baxter.sh
+```
+
+Make necessary changes to `baxter.sh`. See: http://sdk.rethinkrobotics.com/wiki/Workstation_Setup#Step_6:_Configure_Baxter_Communication.2FROS_Workspace
+
+# 3rd-Party Software
+
+This repository makes use of 3rd-party software that falls under a different license from the software developed by Team ACRV. Our license does not apply to 3rd-party software included in this repository and the intended license of the original owners left in place. Modifications made to 3rd-party software have been made in alignment with the governing license. Modifications may not be directly documented.
+
+## 3rd-Party Software List
+
+ * acrv_trac_ik -> https://bitbucket.org/traclabs/trac_ik
+ * apc_gripper_description -> https://github.com/RethinkRobotics/baxter_common
+ * baxter_description -> https://github.com/RethinkRobotics/baxter_common
+ * baxter_moveit -> https://github.com/ros-planning/moveit_robots
+ * rethink_ee_description -> https://github.com/RethinkRobotics/baxter_common
+ * apc_random_orders -> https://github.com/Jorge-C/apc_random_orders
+ * apc_kinfu -> https://github.com/PointCloudLibrary/pcl/tree/master/gpu/kinfu
+ * ros_realsense -> https://github.com/jamessergeant/ros_realsense
diff --git a/acrv_tracik/README.md b/acrv_tracik/README.md
@@ -0,0 +1,63 @@
+The ROS packages in this repository were created to provide an alternative
+Inverse Kinematics solver to the popular inverse Jacobian methods in KDL.
+Specifically, KDL's convergence algorithms are based on Newton's method, which
+does not work well in the presence of joint limits --- common for many robotic
+platforms.  TRAC-IK concurrently runs two IK implementations.  One is a simple
+extension to KDL's Newton-based convergence algorithm that detects and
+mitigates local minima due to joint limits by random jumps.  The second is an
+SQP (Sequential Quadratic Programming) nonlinear optimization approach which
+uses quasi-Newton methods that better handle joint limits.  By default, the IK
+search returns immediately when either of these algorithms converges to an
+answer.  Secondary constraints of distance and manipulability are also provided 
+in order to receive back the "best" IK solution.
+
+###This repo contains 4 ROS packages:###
+
+- trac\_ik is a metapackage with build and [Changelog](https://bitbucket.org/traclabs/trac_ik/src/HEAD/trac_ik/CHANGELOG.rst) info.
+
+- trac\_ik\_examples contains examples on how to use the standalone TRAC-IK library.
+
+- [trac\_ik\_lib](https://bitbucket.org/traclabs/trac_ik/src/HEAD/trac_ik_lib), the TRAC-IK kinematics code,
+builds a .so library that can be used as a drop in replacement for KDL's IK
+functions for KDL chains. Details for use are in trac\_ik\_lib/README.md.
+
+- [trac\_ik\_kinematics\_plugin](https://bitbucket.org/traclabs/trac_ik/src/HEAD/trac_ik_kinematics_plugin) builds a [MoveIt! plugin](http://moveit.ros.org/documentation/concepts/#kinematics) that can
+replace the default KDL plugin for MoveIt! with TRAC-IK for use in planning.
+Details for use are in trac\_ik\_kinematics\_plugin/README.md. (Note prior to v1.1.2, the plugin was not thread safe.)
+
+###A detailed writeup on TRAC-IK can be found here:###
+
+[Humanoids-2015](https://personal.traclabs.com/~pbeeson/publications/b2hd-Beeson-humanoids-15.html) (reported results are from v1.0.0 of TRAC-IK, see below for newer results).
+
+###Some sample results are below: 
+
+_Orocos' **KDL**_ (inverse Jacobian w/ joint limits), _**KDL-RR**_ (our fixes to KDL joint limit handling), and _**TRAC-IK**_ (our concurrent inverse Jacobian and non-linear optimization solver; Speed mode) are compared below.
+
+IK success and average speed (for successful solves) as of TRAC-IK tag v1.4.1.  All results are from 10,000 randomly generated, reachable joint configurations.  Full 3D pose IK was requested at 1e-5 Cartesian error for x,y,z,roll,pitch,yaw with a maximum solve time of 5 ms.  All IK queries are seeded from the chain's "nominal" pose midway between joint limits.
+
+**Note on success**: Neither KDL nor TRAC-IK uses any mesh information to determine if _valid_ IK solutions result in self-collisions.  IK solutions deal with link distances and joint ranges, and remain agnostic about self-collisions due to volumes.  Expected future enhancements to TRAC-IK that search for multiple solutions may also include the ability to throw out solutions that result in self collisions (provided the URDF has valid geometry information); however, this is currently not the behaviour of any generic IK solver examined to date.
+
+**Note on timings**: The timings provided include both successful and unsuccessful runs.  When an IK solution is not found, the numerical IK solver implementations will run for the full timeout requested, searching for an answer; thus for robot chains where KDL fails much of the time (e.g., Jaco-2), the KDL times are skewed towards the user requested timeout value (here 5 ms).  
+
+Chain | DOFs | Orocos' _KDL_ solve rate | Orocos' _KDL_ Avg Time | _KDL-RR_ solve rate | _KDL-RR_ Avg Time | _TRAC-IK_ solve rate | _TRAC-IK_ Avg Time
+- | - | - | - | - | - | - | -
+Atlas 2013 arm | 6 | **75.54%** | 1.35ms | **97.13%** | 0.39ms | **99.97%** | 0.33ms
+Atlas 2015 arm | 7 | **75.71%** | 1.50ms | **93.13%** | 0.81ms | **99.18%** | 0.48ms
+Baxter arm | 7 | **61.07%** | 2.21ms | **89.52%** | 1.02ms | **99.17%** | 0.60ms
+Denso VS-068 | 6 | **27.92%** | 3.69ms | **98.13%** | 0.42ms | **99.78%** | 0.38ms
+Fanuc M-430iA/2F | 5 | **21.07%** | 3.99ms | **88.34%** | 0.92ms | **99.16%** | 0.58ms
+Fetch arm | 7 | **92.49%** | 0.73ms | **93.82%** | 0.72ms | **99.96%** | 0.44ms
+Jaco2 | 6 | **26.23%** | 3.79ms | **97.66%** | 0.58ms | **99.51%** | 0.58ms
+KUKA LBR iiwa 14 R820 | 7 | **37.71%** | 3.37ms | **94.02%** | 0.73ms | **99.63%** | 0.56ms
+KUKA LWR 4+ | 7 | **67.80%** | 1.88ms | **95.40%** | 0.62ms | **99.95%** | 0.38ms
+PR2 arm | 7 | **83.14%** | 1.37ms | **86.96%** | 1.27ms | **99.84%** | 0.59ms
+NASA Robonaut2 'grasping leg' | 7 | **61.27%** | 2.29ms | **87.57%** | 1.10ms | **99.31%** | 0.67ms
+NASA Robonaut2 'leg' + waist + arm | 15 | **97.99%** | 0.80ms | **98.00%** | 0.84ms | **99.86%** | 0.79ms
+NASA Robonaut2 arm | 7 | **86.28%** | 1.02ms | **94.26%** | 0.73ms | **99.25%** | 0.50ms
+NASA Robosimian arm | 7 | **61.74%** | 2.44ms | **99.87%** | 0.36ms | **99.93%** | 0.44ms
+TRACLabs modular arm | 7 | **79.11%** | 1.35ms | **95.12%** | 0.63ms | **99.80%** | 0.53ms
+UR10 | 6 | **36.16%** | 3.29ms | **88.05%** | 0.82ms | **99.47%** | 0.49ms
+UR5 | 6 | **35.88%** | 3.30ms | **88.69%** | 0.78ms | **99.55%** | 0.42ms
+NASA Valkyrie arm | 7 | **45.18%** | 3.01ms | **90.05%** | 1.29ms | **99.63%** | 0.61ms
+
+Feel free to [email Patrick](mailto:pbeeson@traclabs.com) if there is a robot chain that you would like to see added above.
diff --git a/acrv_tracik/trac_ik/CHANGELOG.rst b/acrv_tracik/trac_ik/CHANGELOG.rst
@@ -0,0 +1,115 @@
+Change history
+==============
+
+1.4.1 (2016-01-16)
+------------------
+
+ * Fixed weird parsing issue with certain URDFs.
+
+
+1.4.0 (2016-01-16)
+------------------
+
+ * Added example program for using TRAC-IK.
+ * Added ability to get KDL chain structures from TRAC-IK class.
+
+1.3.9 (2016-01-16)
+------------------
+
+ * Added new TRAC-IK constructor that takes URDF location, base link, and tip link, instead of KDL structures.
+ * Some README fixes based on user comments
+
+1.3.8 (2016-01-04)
+------------------
+
+ * Fixed how continuous joints are handled in random restarts
+
+1.3.7 (2016-01-04)
+------------------
+
+ * Fixed bug introduced in 1.3.6 where tip frame was being used for Inverse Jacobian, which was never converging.
+
+1.3.6 (2016-01-04)
+------------------
+
+ * Fixed tolerances to be in tip frame, not base frame.
+ * Fixed Hydro compile issue
+ * Changed to std::numeric_limits for +/-FLT_MAX and epsilons
+ * Fixed potential error with URDF soft limits
+
+1.3.5 (2015-12-08)
+------------------
+
+ * Overhaul of how multiple solutions are managed in TRAC-IK threads.
+ * Improved speed and accurate returning of unique results found.
+
+1.3.4 (2015-12-10)
+------------------
+
+ * Fixed a bug where we were looping over solutions inside of looping over solutions. 
+
+1.3.3 (2015-12-10)
+------------------
+
+ * Change final normalization for manipulability metrics with continuous joints. Make continuous joints explicitly handled (if lower_limit >= upper_limit, joint is continuous).
+
+1.3.2 (2015-12-10)
+------------------
+
+ * Fixed bug where TRAC-IK multisolution modes weren't using the computed random seeds, but were using the same seed over and over.
+ * Fixed potential issues with large bound joints seraching too large of a space
+
+1.3.1 (2015-12-08)
+------------------
+
+ * Slight tweak to Manipulation metrics function to ensure it always works, even with weird Jacobians.
+
+1.3.0 (2015-12-07)
+------------------
+
+ * Added a mode to TRAC-IK constructor that determines how the IK solver runs: 1) Speed return immediately whenever any solution is found (all other modes run for the requested timeout to try to find multiple solutions); 2) Distance runs for the timeout and returns the solution found that minimizes the Sum-of-Squares error from the seed; 3) Manip1 returns the solution that maximizes the manipulation metric sqrt(det(J*J^T)); 4)  Manip2 returns the metric that minimizes the condition number |J||J^-1|.
+ * Added support for these modes in the MoveIt! plugin via the kinematic.yaml parameter solve_type, which can be one of the strings "Speed", "Distance", "Manipulation1", "Manipulation2".
+
+
+1.2.1 (2015-12-07)
+------------------
+
+ * Fixed a bug in the MoveIt! plugin FK call that assumed all joint poses were desired.
+
+
+1.2.0 (2015-12-04)
+------------------
+
+ * Extended TRAC-IK to both run in two ways: 1) the old mode of first IK solution found causes TRAC-IK to return immediately, versus 2) the new mode where TRAC-IK runs for the full requested timeout duration, then sorts all solutions according to distance from the seed and returns the minimum.
+ * Made MoveIt! support this new IK run mode if the user desires.
+ * Improved timing info to use a higher solution clock.
+ * Fixed TRAC-IK's abort/reset of KDL-RR and NLOpt-IK to catch race conditions.
+
+
+1.1.2 (2015-12-3)
+------------------
+
+ * Fixed issue where clamping a seed to be within the joint limits might still have values outside the limits.
+ * Fixed issue where MoveIt! plugin was not thread safe.
+ * Fixed an issue in MoveIt! plugin where error_code passed in uninitialized to SUCCESS could cause IK to say it failed when it did not.
+
+
+1.1.1 (2015-11-19)
+------------------
+
+ * Prepared code to have auto test suite run to generate data in main README.md.
+
+
+1.1.0 (2015-11-12)
+------------------
+
+ * Improvements to KDL-RR that better handle joint limits on rotational joints that can turn +- PI.
+ * Fixed bug where continuous joints could cause problems.
+ * Made NLOpt modes enums instead of integer parameters.
+
+
+1.0.0 (2015-11-10)
+------------------
+
+ * Initial checkin of TRAC-IK as of Humanoids 2015 submission.  Pulled from private repo.
+ * Made trac_ik packages conform to rosdep standards.
diff --git a/acrv_tracik/trac_ik/CMakeLists.txt b/acrv_tracik/trac_ik/CMakeLists.txt
@@ -0,0 +1,4 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(trac_ik)
+find_package(catkin REQUIRED)
+catkin_metapackage()
diff --git a/acrv_tracik/trac_ik/package.xml b/acrv_tracik/trac_ik/package.xml
@@ -0,0 +1,29 @@
+<package>
+
+  <name>trac_ik</name>
+  <version>1.4.1</version>
+  <description>
+    The ROS packages in this repository were created to provide an improved
+    alternative Inverse Kinematics solver to the popular inverse Jacobian
+    methods in KDL.  TRAC-IK handles joint-limited chains better than KDL
+    without increasing solve time.
+  </description> 
+  <maintainer email="pbeeson@traclabs.com">Patrick Beeson</maintainer>
+  <author>Patrick Beeson</author>
+  <author>Barrett Ames</author>
+  <license>BSD</license>
+
+  <url type="website">http://www.ros.org/wiki/trac_ik</url>
+  <url type="repository">https://bitbucket.org/traclabs/trac_ik</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <run_depend>trac_ik_lib</run_depend>
+  <run_depend>trac_ik_kinematics_plugin</run_depend>
+  <run_depend>trac_ik_examples</run_depend>
+
+  <export>
+    <metapackage/>
+  </export>
+
+</package>