high performance logging via FRI + flatbuffers with the kuka iiwa

BasisProject.cmake

@@ -104,7 +104,7 @@ basis_project (
   # dependencies
   DEPENDS
     FlatBuffers # google flatbuffers https://github.com/google/flatbuffers
-    Boost{program_options,filesystem,unit_test_framework,system,regex,coroutine,chrono}
+    Boost{program_options,filesystem,unit_test_framework,system,regex,coroutine,chrono,iostreams}
     #<dependency>
   OPTIONAL_DEPENDS
     #<optional-dependency>
@@ -119,6 +119,7 @@ basis_project (
     sch-core           # Algorithms for convex hulls https://github.com/jrl-umi3218/sch-core used for inverse kinematics
     RBDyn              # Models the dynamics of rigid body system https://github.com/jrl-umi3218/RBDyn used for inverse kinematics
     Tasks              # Real time control of Kinematic Trees https://github.com/jrl-umi3218/Tasks used for inverse kinematics
+    mc_rbdyn_urdf      # This library allows to parse an URDF file and create RBDyn structure from it. https://github.com/jrl-umi3218/mc_rbdyn_urdf
     spdlog             # fast logging library https://github.com/gabime/spdlog
     LibDL              # Linux Dynamic Loader library, linux only https://refspecs.linuxfoundation.org/LSB_2.0.1/LSB-Core/LSB-Core/libdl.html
   TEST_DEPENDS

config/FindFlatBuffers.cmake

@@ -117,12 +117,12 @@ if(FLATBUFFERS_FOUND)
       set(ABSOLUTE_FBS_FILE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/${FLATBUFFERS_DIR}/${FILE})
 
       add_custom_command(OUTPUT ${FLATC_OUTPUT}
-        COMMAND ${FLATBUFFERS_FLATC_EXECUTABLE}
+        COMMAND ${FLATBUFFERS_COMPILER}
         # Note: We are setting several custom parameters here to make life easier.
         # see flatbuffers documentation for details.
         # flatc --gen-name-strings --scoped-enums --gen-object-api -c -j -p -o
         # see https://google.github.io/flatbuffers/flatbuffers_guide_using_schema_compiler.html
-        ARGS --gen-name-strings --scoped-enums --gen-object-api -c -j -p -o "${OUTPUT_DIR}" ${FILE}
+        ARGS --gen-name-strings --scoped-enums --gen-object-api -c -j -p -o "${OUTPUT_DIR}" ${ABSOLUTE_FBS_FILE_PATH}
 		    MAIN_DEPENDENCY ${ABSOLUTE_FBS_FILE_PATH}
         COMMENT "Building C++, Java, and Python header for ${FILE}"
         WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/${FLATBUFFERS_DIR})

config/FindFlatBuffers.cmake.deleteme

@@ -0,0 +1,160 @@
+#
+# 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.
+#
+# Tries to find Flatbuffers headers and libraries.
+#
+# Usage of this module as follows:
+#
+#  find_package(Flatbuffers)
+#
+# Variables used by this module, they can change the default behaviour and need
+# to be set before calling find_package:
+#
+#  Flatbuffers_HOME -
+#   When set, this path is inspected instead of standard library locations as
+#   the root of the Flatbuffers installation.  The environment variable
+#   FLATBUFFERS_HOME overrides this veriable.
+#
+# This module defines
+#  FLATBUFFERS_INCLUDE_DIR, directory containing headers
+#  FLATBUFFERS_LIBS, directory containing flatbuffers libraries
+#  FLATBUFFERS_STATIC_LIB, path to libflatbuffers.a
+#  FLATBUFFERS_FOUND, whether flatbuffers has been found
+
+if( NOT "$ENV{FLATBUFFERS_HOME}" STREQUAL "")
+file( TO_CMAKE_PATH "$ENV{FLATBUFFERS_HOME}" _native_path )
+list( APPEND _flatbuffers_roots ${_native_path} )
+elseif ( Flatbuffers_HOME )
+list( APPEND _flatbuffers_roots ${Flatbuffers_HOME} )
+endif()
+
+# Try the parameterized roots, if they exist
+if ( _flatbuffers_roots )
+find_path( FLATBUFFERS_INCLUDE_DIR NAMES flatbuffers/flatbuffers.h
+    PATHS ${_flatbuffers_roots} NO_DEFAULT_PATH
+    PATH_SUFFIXES "include" )
+find_library( FLATBUFFERS_LIBRARIES NAMES flatbuffers
+    PATHS ${_flatbuffers_roots} NO_DEFAULT_PATH
+    PATH_SUFFIXES "lib" )
+else ()
+find_path( FLATBUFFERS_INCLUDE_DIR NAMES flatbuffers/flatbuffers.h )
+find_library( FLATBUFFERS_LIBRARIES NAMES flatbufferFLATBUFFERS_FLATC_EXECUTABLEs )
+endif ()
+
+find_program(FLATBUFFERS_COMPILER flatc
+PATH
+$ENV{FLATBUFFERS_HOME}/bin
+${_flatbuffers_roots}/bin
+/usr/local/bin
+/usr/bin
+NO_DEFAULT_PATH
+)
+
+if (FLATBUFFERS_INCLUDE_DIR AND FLATBUFFERS_LIBRARIES)
+set(FLATBUFFERS_FOUND TRUE)
+get_filename_component( FLATBUFFERS_LIBS ${FLATBUFFERS_LIBRARIES} PATH )
+set(FLATBUFFERS_LIB_NAME libflatbuffers)
+set(FLATBUFFERS_STATIC_LIB ${FLATBUFFERS_LIBS}/${FLATBUFFERS_LIB_NAME}.a)
+else ()
+set(FLATBUFFERS_FOUND FALSE)
+endif ()
+
+if (FLATBUFFERS_FOUND)
+if (NOT Flatbuffers_FIND_QUIETLY)
+message(STATUS "Found the Flatbuffers library: ${FLATBUFFERS_LIBRARIES}")
+endif ()FLATBUFFERS_COMPILER
+else ()
+if (NOT Flatbuffers_FIND_QUIETLY)
+set(FLATBUFFERS_ERR_MSG "Could not find the Flatbuffers library. Looked in ")
+if ( _flatbuffers_roots )
+  set(FLATBUFFERS_ERR_MSG "${FLATBUFFERS_ERR_MSG} in ${_flatbuffers_roots}.")
+else ()
+  set(FLATBUFFERS_ERR_MSG "${FLATBUFFERS_ERR_MSG} system search paths.")
+endif ()
+if (Flatbuffers_FIND_REQUIRED)
+  message(FATAL_ERROR "${FLATBUFFERS_ERR_MSG}")
+else (Flatbuffers_FIND_REQUIRED)
+  message(STATUS "${FLATBUFFERS_ERR_MSG}")
+endif (Flatbuffers_FIND_REQUIRED)
+endif ()
+endif ()
+
+mark_as_advanced(
+FLATBUFFERS_INCLUDE_DIR
+FLATBUFFERS_LIBS
+FLATBUFFERS_STATIC_LIB
+FLATBUFFERS_COMPILER
+)
+
+if(FLATBUFFERS_FOUND)
+  function(FLATBUFFERS_GENERATE_C_HEADERS Name FLATBUFFERS_DIR OUTPUT_DIR)
+  # Name is the name of the user defined variable that will be created by this function
+  #     Another variable that will be set is ${NAME}_OUTPUTS which will be set to the names
+  #     of all output files that have been generated.FLATBUFFERS_COMPILER
+  # FLATBUFFERS_DIR is the directory in which to look for the .fbs files
+  # OUTPUT_DIR is the directory in which all output files should be placed
+    set(FLATC_OUTPUTS)
+    foreach(FILE ${ARGN})
+      get_filename_component(FLATC_OUTPUT ${FILE} NAME_WE)
+      # create a target for the specific flatbuffers file
+      set(FBS_FILE_COPY_INCLUDE copy_${FLATC_OUTPUT}_to_include)
+      set(FBS_FILE_COPY_BIN copy_${FLATC_OUTPUT}_to_bin)
+      # create a target for the generated output cpp file
+      set(FLATC_OUTPUT
+        "${OUTPUT_DIR}/${FLATC_OUTPUT}_generated.h")
+        list(APPEND FLATC_OUTPUTS ${FLATC_OUTPUT} ${FBS_FILE_COPY_INCLUDE} ${FBS_FILE_COPY_BIN})
+
+      # this is the absolute path to the actual filename.fbs file
+      set(ABSOLUTE_FBS_FILE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/${FLATBUFFERS_DIR}/${FILE})
+
+      add_custom_command(OUTPUT ${FLATC_OUTPUT}
+        COMMAND ${FLATBUFFERS_COMPILER}
+        # Note: We are setting several custom parameters here to make life easier.
+        # see flatbuffers documentation for details.
+        # flatc --gen-name-strings --scoped-enums --gen-object-api -c -j -p -o
+        # see https://google.github.io/flatbuffers/flatbuffers_guide_using_schema_compiler.html
+        ARGS --gen-name-strings --scoped-enums --gen-object-api -c -j -p -o "${OUTPUT_DIR}" ${FILE}
+		    MAIN_DEPENDENCY ${ABSOLUTE_FBS_FILE_PATH}
+        COMMENT "Building C++, Java, and Python header for ${FILE}"
+        WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/${FLATBUFFERS_DIR})
+
+         # need to copy the flatbuffers schema so config files can be loaded
+         # http://stackoverflow.com/a/13429998/99379
+         # CMAKE_CURRENT_SOURCE_DIR 
+         #    this is the directory where the currently processed CMakeLists.txt is located in
+      # terminal copy commands change between OS versions, so we use CMake's built in file
+      # copy command which runs with "cmake -E copy file_to_copy file_destination"
+      # we use some variables here so the path is reproducible.
+	     add_custom_command(OUTPUT ${FBS_FILE_COPY_INCLUDE}
+          COMMAND ${CMAKE_COMMAND} ARGS -E copy ${ABSOLUTE_FBS_FILE_PATH} ${OUTPUT_DIR}
+          MAIN_DEPENDENCY ${CMAKE_CURRENT_SOURCE_DIR}/${FLATBUFFERS_DIR}/${FILE}
+          COMMENT "Copying fbs file ${FILE} to ${OUTPUT_DIR}"
+          WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/${FLATBUFFERS_DIR}
+       )
+
+       add_custom_command(OUTPUT ${FBS_FILE_COPY_BIN}
+       # TODO(ahundt) remove hacky /bin manually set path, this will break for some IDEs
+       COMMAND ${CMAKE_COMMAND} ARGS -E copy ${ABSOLUTE_FBS_FILE_PATH} ${CMAKE_BINARY_DIR}/bin
+       MAIN_DEPENDENCY ${CMAKE_CURRENT_SOURCE_DIR}/${FLATBUFFERS_DIR}/${FILE}
+       COMMENT "Copying fbs file ${FILE} to ${CMAKE_BINARY_DIR}"
+       WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/${FLATBUFFERS_DIR}
+       )
+    endforeach()
+    set(${Name}_OUTPUTS ${FLATC_OUTPUTS} PARENT_SCOPE)
+  endfunction()
+
+  set(FLATBUFFERS_INCLUDE_DIRS ${FLATBUFFERS_INCLUDE_DIR})
+  include_directories(${CMAKE_BINARY_DIR})
+else()
+  set(FLATBUFFERS_INCLUDE_DIR)
+endif()

config/Settings.cmake

@@ -42,7 +42,9 @@ if(UNIX AND CMAKE_SYSTEM_PROCESSOR MATCHES "x86_64")
 	ADD_DEFINITIONS(-fPIC)
 endif(UNIX AND CMAKE_SYSTEM_PROCESSOR MATCHES "x86_64")
 
-
+if(UNIX AND NOT APPLE)
+    set(LINUX_ONLY_LIBS ${LIBDL_LIBRARIES})
+endif()
 # Link the boost.log library
 # @todo consider an alternative to always linking boost log
 ADD_DEFINITIONS(-DBOOST_LOG_DYN_LINK)

doc/howto/DataRecordingAndPlayback.rst

@@ -0,0 +1,110 @@
+====================
+Data Analysis 
+====================
+
+.. note:: I will interpret the data files, including the relative location and the main content.
+
+How to collect data
+==================================
+
+In this project, the messages are communicated by `FlatBuffers <https://google.github.io/flatbuffers/>`__  between PC and Kuka workstation, which makes reading and writing data efficient. 
+
+
+1. Install `VREP <http://coppeliarobotics.com/>`__ and `grl <https://github.com/ahundt/robotics_setup>`__.
+
+2. Start VREP and load the scene `RoboneSimulation_private.ttt <https://github.com/ahundt/robonetracker/blob/master/modules/roboneprivate/data/RoboneSimulation_private.ttt>`__.
+
+3. Set flatbuffer limit in `robone.lua <https://github.com/ahundt/robonetracker/blob/master/modules/grl/src/lua/robone.lua>`__.
+   The hard limit for flatbuffer is 2 GB, but you can customize it based on your requirement in this project. 
+   When flatbuffer size hits this limit, the data will be written to disk. Or when you click on the STOP button in VREP, 
+   the data will also be written to disk automaticly, regardless of the limit::
+
+   KUKA_single_buffer_limit_bytes = 256    -- MB
+   FB_single_buffer_limit_bytes = 1024     -- MB
+
+4. Start to record data while simulation is running in `robone.lua <https://github.com/ahundt/robonetracker/blob/master/modules/grl/src/lua/robone.lua>`__.
+   By defaut, program starts to collect data automatically. You can change it by following functions::
+      simExtKukaLBRiiwaRecordWhileSimulationIsRunning(true, KUKA_single_buffer_limit_bytes)
+	  simExtAtracsysFusionTrackRecordWhileSimulationIsRunning(true, FB_single_buffer_limit_bytes)
+
+5. Enable lua scripts StartRealArmDriverScript and Tracker in VREP, then program can communicate with Atracsys and Kuka.
+   Only after connecting the devices, it can start to record data correctly.
+
+6. Stop recording data.
+   There are two ways to stop the data collection process, one is the buffer hits the limit in Step 3, the other is to click on the STOP button in VREP, 
+   then it will stop and write the data to disk automatically.
+
+
+The binary files are put in VREP data folder (i.e. ~/src/V-REP_PRO_EDU_V3_4_0_Linux/data/), 
+and are named by the time stamp (i.e. 2018_03_26_19_06_21_FusionTrack.flik, 2018_03_26_19_06_21_Kukaiiwa.iiwa).
+
+The command to generate json file is as below, the binary file and the fbs file should be put in the same folder::
+    flatc -I . --json LogKUKAiiwaFusionTrack.fbs -- 2018_03_26_19_06_21_FusionTrack.flik
+    flatc -I . --json KUKAiiwa.fbs -- 2018_03_26_19_06_21_Kukaiiwa.iiwa
+
+How to export data
+==================================
+
+When exporting the flatbuffer file to CSV, you need to follow the instructions below:
+
+1. Keep the binary files in the VREP data folder (i.e. ~/src/V-REP_PRO_EDU_V3_4_0_Linux/data/);
+
+2. Run `readFlatbufferTest <https://github.com/ahundt/robonetracker/tree/master/modules/grl/test>`__ in terminal with the arguments of the name of binary file::
+
+        ./readFlatbufferTest 2018_03_26_19_06_21_Kukaiiwa.iiwa 2018_03_26_19_06_21_FusionTrack.flik
+	    # You can also pass the arguments manually to the main() function in readFlatbufferTest.cpp.
+
+3. The generated csv files will be put into the VREP data folder  in which the subfolder is named by time stamp.
+    Label explanation:
+	.. code-block:: bash
+        local_request_time_offset: PC time of sending a requesting command to devices;
+	    local_receive_time_offset: PC time of receiving measured data from devices;
+	    device_time_offset: the time from device;
+	    time_Y: time driftting, device_time_offset - local_request_time_offset;
+	    counter: the identifier of message, defined by devices;
+	    X	Y	Z	A	B	C: the cartesian postion and oritation in Plucker coordinate system;
+	    M_Pos_J*: measured joint position of joint i from kuka;
+	    C_Pos_J*: command joint position of joint i to kuka;
+	    M_Tor_J*: measured joint torque of joint i form kuka;
+	    C_Tor_J*: comand joint torque of joint i to kuka;
+	    E_Tor_J*: external torque of joint i exerted on kuka;
+
+		# on the first message save local_request_time as the initial_local_request_time. Both kuka and Atracsys share the same initial_local_request_time, which means they have the same time axis.
+        # on the first message save device_time as the initial_device_time
+        X = local_request_time
+        local_request_offset = (local_request_time - initial_local_request_time)
+        device_offset = (device_time - initial_device_time)
+        time_Y = device_offset - local_request_offset
+
+	CSV file explanation:
+	   FTKUKA_TimeEvent.csv has the information from both Atracsys and kuka. It can help analysis time event.
+	   FT_Pose_Marker*.csv have the time event and pose (in Plucker coordinate) of the specific marker in Atracsys space;
+	   FT_TimeEvent.csv gives more detail information about time event from Atracsys, such as the time step;
+	   KUKA_FRIMessage.csv includes all the FRI message from robot;
+	   KUKA_Command_Joint.csv has the commanding joint angle sent to robot, which should use local_request_time_offset as time axis when plotting;
+	   KUKA_Measured_Joint.csv has the measured joint angles received from robt, which should use local_receive_time_offset as time axis when plotting. 
+	   KUKA_TimeEvent.csv gives more detail information about time event from kuka, such as the time step;
+
+	   All the CSV files above are generated from binary files. To make it convenient, all the files have time event information, which can be used as X-axis when plotting. 
+
+
+Replay Process
+==================================
+The replay process can perform the forward kinematics to get the cartesian pose of the end effector.
+
+1. Copy the KUKA_Measured_Joint.csv, KUKA_Command_Joint.csv and FT_Pose_Marker22.csv to the  ~/src/V-REP_PRO_EDU_V3_4_0_Linux/data/data_in/.
+The result will be writen in ForwardKinematics_Pose.csv.
+
+2. Enable the CutBoneScript.
+
+3. Set the parameter of simExtGrlInverseKinematicsStart(...) to replay_mode in robone.lua.
+   You should run this function two times, one time commanddata is true, the other is false. Then you can get the cartesian pose for both command and measured data::
+
+		-- ik_mode, run real inverse kinematics algorith;
+		-- replay_mode, run the replay process;
+		-- test_mode, go to a test pose;
+		-- commanddata, only in replay_mode we need to set it to select the joint data set.
+		commanddata = false
+		run_mode = { ik_mode = 1, replay_mode = 2, test_mode = 3}
+		print("Moving Robotiiwa arm along inversekinematics")
+        simExtGrlInverseKinematicsStart(run_mode.replay_mode, commanddata)