naveenoid / kumonoito

The silken spider thread that lets one go back and forth between Drake and ROS.

Kumonoito

Kumonoito is the silken spidery thread that takes one from Drake to ROS and back. This thread, cast by the Great Warrior Gesar (Russ Tedrake) was woven in the hidden land of Drake-Shambhala where Buddhist values and traditions rule and can lead the hordes of the righteous but damned (Drake Developers) to climb up towards salvation (ROS / LCM interoperability).

More practically, this repository exemplifies Drake simulations and systems that communicate via ROS and LCM messages.

Organisation, Setup and Install

Currently, this repo is setup with 4 Drake Systems that enable ROS messaging communication : A RosPublisherSystem, a RosSubscriberSystem, a RosJointStatePublisher, and a TfPublisherSystem. It also includes a couple of simulation demos that utilize these systems in various ways and the associated models needed to run these simulations.

Kumonoito is currently developed for and tested with ROS Kinetic and Ubuntu 16.04

Install

Kumonoito depends on ROS and Drake.

1. Install ROS Kinetic.
2. Install Drake as a library. Kumonoito is currently tested with a locally built Drake library. To build Drake as a library,
   • Navigate to $<drake-dir>
   • Run bazel run //:install -- $<drake_lib_dir>
3. Clone Kumonoito:
   • Create a catkin workspace somewhere is your filesystem:
     • source /opt/ros/kinetic/setup.bash
     • mkdir -p ~/catkin_ws/src
     • catkin_make
   • Download Kumonoito repository into the catkin workspace
     • cd ~/catkin_ws/src/
     • git clone https://github.com/naveenoid/kumonoito.git
4. Build catkin packages:
   • cd ~/catkin_ws && catkin_make install

Demo Description / Setup

There are 2 demos available by default in Kumonoito and they are to some extent meant to be exemplary. These demos are built around the "Robotable" setup that consists of 2 Kinova Jaco Arms mounted to one side of a custom table. A few objects of interest for manipulation experiments are placed on the surface. The two projects are a (i) Passive "Monolithic" Sim of 2 Kinova Arms, and a (ii) Controlled RoboTable sim that responds to trajectory commands via ROS Messages (Note that controlled version message listening version is WIP - it currently is a monolithic style demo).

Demo Setup

1. Install Kinova ROS Package (Note that only the kinova_description project is required at this stage).
   • git clone https://github.com/Kinovarobotics/kinova-ros /tmp/kinova-ros
   • cd ~/catkin_ws/src/
   • cp -a /tmp/kinova-ros/kinova_description/ .
   • cd ~/catkin_ws && catkin_make install

Demo Execution

1. Setup enviroment variables and extra ROS packages
   • DRAKE_RESOURCE_DIR export DRAKE_RESOURCE_ROOT=<drake_lib_dir>/drake/
   • ROS and catkin source /opt/ros/kinetic/setup.bash and source ~/catkin_ws/install/setup.bash
   • Install auxiliary packages apt-get install -y ros-$ROS_DISTRO-rviz ros-$ROS_DISTRO-robot-state-publisher
2. For launching the passive demo:
   • roslaunch <catkin_ws>/kumonoito/launch/passive_multi_jaco_sim.launch
   • Once launched, if there are no problems, on the rviz window you should see the robot-table visualized on rviz and the 2 kinova Jaco arms falling passively. Various objects have been placed on the table.
3. For launching the controlled demo:
   • roslaunch <catkin_ws>/kumonoito/launch/controlled_robot_table_sim.launch
   • if there are no problems, on the rviz window you should see the robot-table visualized on rviz and the 2 kinova Jaco arms move to an arbitrarily specified position in space. Various objects have been placed on the table.

Special note for both demos: Both demos simply publish to 1 kinova arm and the second arm uses a remap of the appropriate messages to render the same behaviour. This has been set for purely cosmetic reasons and a version with 2 independently controlled arms is WIP.

kumonoito's Issues

stable master feature list

For a stable master version from which devs can start working off on their branches, the following set of features are targeted :

• Generic ROS message pub/sub systems : The systems have abstract input / output ports respectively and can be used to pub/sub templated ROS messages in the specified nodes.
• Generic TF publisher : An equivalent of the drake visualizer - publishes a "flat" tf with each RigidBody's pose.
• Passive robot demo : shows 1 / 2 Kinova robot arms that fall passively and the simulation can be visualized in both RVIZ and DrakeVisualizer.
• Controlled robot monolithic demo : shows 1 / 2 Kinova robot arms mounted on the robot-table that are position controlled.
• Controlled robot demo : shows 1 / 2 Kinova robot arms that are listening to control messages coming from a seperate process that commands positions. The simulation can be visualized in both RVIZ and DrakeVisualizer and the RobotState can be plotted out in Rqtplot etc.

stable branch/tag? instructions for the execution?

Just a question: is the current master branch the reference branch for testing the example? In that case, how is the demo supposed to be executed?

Thanks.

Problems linking octomap: /usr/bin/ld: cannot find -loctomap::octomap

The current master branch fails to compile with the following error:

[ 18%] Linking CXX shared library /home/jrivero/code/TRI/ws/devel/.private/kumonoito/lib/libjaco_common.so
/usr/bin/cmake -E cmake_link_script CMakeFiles/jaco_common.dir/link.txt --verbose=1
/usr/bin/c++  -fPIC   -shared -Wl,-soname,libjaco_common.so -o /home/jrivero/code/TRI/ws/devel/.private/kumonoito/lib/libjaco_common.so CMakeFiles/jaco_common.dir/src/jaco_common.cc.o /tmp/drake/lib/libdrake.so /tmp/drake/lib/libBulletCollision.so /tmp/drake/lib/libLinearMath.so /tmp/drake/lib/libfcl.so /tmp/drake/lib/libccd.so -loctomap::octomap /tmp/drake/lib/libignition_rndf.so /tmp/drake/lib/liblcm.so -lglib-2.0 -lpthread /tmp/drake/lib/libprotobuf.so /tmp/drake/lib/libyaml_cpp.so /tmp/drake/lib/libfmt.so /tmp/drake/lib/libscsdir.so /tmp/drake/lib/libsdformat.so /tmp/drake/lib/libignition_math.so -ltinyxml /tmp/drake/lib/libtinyobjloader.so -ltinyxml2 -Wl,-rpath,/tmp/drake/lib 
/usr/bin/ld: cannot find -loctomap::octomap
collect2: error: ld returned 1 exit status

The problem comes from the fcl cmake module generated by Drake which list in the INTERFACE_LINK_LIBRARIES. For some reason the library target name "octomap::octomap" is not being resolved and is passed to the linker as it.

Since libfcl (the consumer of octomap) does link statically against ccd and octomap, there is not need to supply them in the cmake module. A dirty patch to solve the issue:

--- /opt/drake/lib/cmake/fcl/fcl-config.cmake	2017-11-29 07:43:37.000000000 +0100
+++ /tmp/drake/lib/cmake/fcl/fcl-config.cmake	2017-11-29 19:37:43.848797013 +0100
@@ -93,7 +93,6 @@
 set_target_properties(fcl::fcl PROPERTIES
   IMPORTED_LOCATION "${${CMAKE_FIND_PACKAGE_NAME}_IMPORT_PREFIX}/lib/libfcl.so"
   INTERFACE_INCLUDE_DIRECTORIES "${${CMAKE_FIND_PACKAGE_NAME}_IMPORT_PREFIX}/include"
-  INTERFACE_LINK_LIBRARIES "ccd::ccd;Eigen3::Eigen;octomap::octomap"
 )
 
 set(fcl_LIBRARIES "fcl::fcl")

I think that fixing the real problem does not worth the effort since these modules are going to be removed by RobotLocomotion/drake#7417. I've confirmed that building drake using the branch in the PR makes this package to successfully compile.