diff --git a/BasisProject.cmake b/BasisProject.cmake index dc9e90a..4ebafbf 100644 --- a/BasisProject.cmake +++ b/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} # OPTIONAL_DEPENDS # @@ -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 diff --git a/README.md b/README.md index 47b5230..633da55 100644 --- a/README.md +++ b/README.md @@ -7,6 +7,9 @@ The Generic Robotics Library (GRL) has a long term goal of implementing robotics Currently GRL implements C++11 drivers for the new Kuka LBR iiwa arm and hardware integration with [ROS](ros.org) and the [V-REP](http://http://www.coppeliarobotics.com/index.html) robotics simulation software. +[![grl kuka control from linux over Java API demo](https://img.youtube.com/vi/pvs-lG2_K3g/0.jpg)](https://youtu.be/pvs-lG2_K3g) + +This demo video is of the KUKA iiwa robot following a path using GRL. What appears to be a "wobbling" motion is actually a highly precise pre-planned path consisting of tiny, nearly concentric circles relative to an object. If you use GRL in research please consider providing a citation: [![DOI](https://zenodo.org/badge/33050653.svg)](https://zenodo.org/badge/latestdoi/33050653) diff --git a/config/FindFlatBuffers.cmake.deleteme b/config/FindFlatBuffers.cmake.deleteme new file mode 100644 index 0000000..6f3a326 --- /dev/null +++ b/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() diff --git a/config/Settings.cmake b/config/Settings.cmake index 804cb12..a013442 100644 --- a/config/Settings.cmake +++ b/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) diff --git a/doc/howto/DataRecordingAndPlayback.rst b/doc/howto/DataRecordingAndPlayback.rst new file mode 100644 index 0000000..a390be3 --- /dev/null +++ b/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 `__ between PC and Kuka workstation, which makes reading and writing data efficient. + + +1. Install `VREP `__ and `grl `__. + +2. Start VREP and load the scene `RoboneSimulation_private.ttt `__. + +3. Set flatbuffer limit in `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 `__. + 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 `__ 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) \ No newline at end of file diff --git a/example/fusionTrackExample.cpp b/example/fusionTrackExample.cpp index 2c36c0e..e254ca1 100644 --- a/example/fusionTrackExample.cpp +++ b/example/fusionTrackExample.cpp @@ -34,7 +34,7 @@ int main(int argc, char **argv) const std::size_t MegaByte = 1024*1024; // If we write too large a flatbuffer - const std::size_t single_buffer_limit_bytes = 512*MegaByte; + const std::size_t single_buffer_limit_bytes = 1024*MegaByte; // Install a signal handler to catch a signal when CONTROL+C std::signal(SIGINT, signal_handler); // std::raise(SIGINT); @@ -87,11 +87,11 @@ int main(int argc, char **argv) std::string json_file_prefix = "test_text_"; std::string json_file_suffix = ".json"; std::string fbs_filename("LogKUKAiiwaFusionTrack.fbs"); - // std::string includePath = grl::getpathtofbsfile(fbs_filename); - std::string includePath = "/home/chunting/src/robonetracker/build/"; + std::size_t builder_size_bytes = 0; - while(!signalStatusG && test) + + while(!signalStatusG && test && builder_size_bytes < single_buffer_limit_bytes) { // loop through all connected devices for(auto serialNumber : serialNumbers) @@ -124,12 +124,9 @@ int main(int argc, char **argv) // also log all other data *except* when there is no new frame available oneKUKAiiwaFusionTrackMessage = grl::toFlatBuffer(fbb, ft, frame, writeParameters); KUKAiiwaFusionTrackMessage_vector.push_back(oneKUKAiiwaFusionTrackMessage); - - // test_oneKUKAiiwaFusionTrackMessage = grl::toFlatBuffer(test_fbb, ft, frame); - // test_KUKAiiwaFusionTrackMessage_vector.push_back(test_oneKUKAiiwaFusionTrackMessage); } - std::size_t builder_size_bytes = fbb.GetSize(); + builder_size_bytes = fbb.GetSize(); if(debug || update_step % print_status_period == 0) { std::size_t newData = builder_size_bytes - previous_size; @@ -143,7 +140,6 @@ int main(int argc, char **argv) { bool success = grl::FinishAndVerifyBuffer(fbb, KUKAiiwaFusionTrackMessage_vector); std::cout << "verifier success " << buffer_num << " : "<< success << std::endl; - // test_binary.fltk should now be named test_binary_0.fltk, test_binary_1.fltk, etc... std::string binary_file_path = binary_file_prefix + std::to_string(buffer_num) + binary_file_suffix; std::string json_file_path = json_file_prefix + std::to_string(buffer_num) + json_file_suffix; std::cout << "Reached single buffer capacity limit of " << static_cast(single_buffer_limit_bytes)/MegaByte << @@ -167,7 +163,7 @@ int main(int argc, char **argv) // with update calls, so yield processor time with the // shortest possible sleep. If you call as fast as is possible // they will write to their .log file, causing all sorts of - // slowdowns and writing huge files to disk very fast. + // slowdowns and other problems. std::this_thread::yield(); } @@ -191,6 +187,7 @@ int main(int argc, char **argv) std::string binary_file_path = binary_file_prefix + std::to_string(buffer_num) + binary_file_suffix; std::string json_file_path = json_file_prefix + std::to_string(buffer_num) + json_file_suffix; std::cout << " fbb.GetSize(): " << fbb.GetSize() << std::endl; + std::cout << "FinishAndVerifyBuffer: " << success << std::endl; success = success && grl::SaveFlatBufferFile( fbb.GetBufferPointer(), @@ -200,16 +197,7 @@ int main(int argc, char **argv) json_file_path); - std::cout << "Saved binary_file_path: " << binary_file_path << " json_file_path: " << json_file_path << " saved with success result: " << success << std::endl; - - // flatbuffers::Offset>> test_states = grl::toFlatBuffer(test_fbb, test_KUKAiiwaFusionTrackMessage_vector); - // flatbuffers::Offset test_fbLogKUKAiiwaFusionTrack = grl::toFlatBuffer(test_fbb, test_states); - - // test_fbb.Finish(test_fbLogKUKAiiwaFusionTrack); - // auto test_verifier = flatbuffers::Verifier(test_fbb.GetBufferPointer(), test_fbb.GetSize()); - // bool test_success = test_verifier.VerifyBuffer(); - // std::cout <<" verifier test_success for LogKUKAiiwaFusionTrack: " << test_success << std::endl; - + std::cout << "Saved binary_file_path: " << binary_file_path << " json_file_path: " << json_file_path << " saved with success result: " << success << std::endl; std::cout << "End of the program" << std::endl; return success; } // End of main function diff --git a/include/grl/flatbuffer/CSVIterator.hpp b/include/grl/flatbuffer/CSVIterator.hpp new file mode 100644 index 0000000..089ab94 --- /dev/null +++ b/include/grl/flatbuffer/CSVIterator.hpp @@ -0,0 +1,77 @@ +#ifndef GRL_CSV_ITERATOR +#define GRL_CSV_ITERATOR +/// Read and parse CSV files in C++ +/// See https://stackoverflow.com/questions/1120140/how-can-i-read-and-parse-csv-files-in-c +#include +#include +#include +#include +namespace grl { +class CSVRow +{ + public: + std::string const& operator[](std::size_t index) const + { + return m_data[index]; + } + std::size_t size() const + { + return m_data.size(); + } + void readNextRow(std::istream& str) + { + std::string line; + std::getline(str, line); + + std::stringstream lineStream(line); + std::string cell; + + m_data.clear(); + while(std::getline(lineStream, cell, ',')) + { + m_data.push_back(cell); + } + // This checks for a trailing comma with no data after it. + if (!lineStream && cell.empty()) + { + // If there was a trailing comma then add an empty element. + m_data.push_back(""); + } + } + private: + std::vector m_data; +}; + +std::istream& operator>>(std::istream& str, CSVRow& data) +{ + data.readNextRow(str); + return str; +} + +class CSVIterator +{ + public: + typedef std::input_iterator_tag iterator_category; + typedef CSVRow value_type; + typedef std::size_t difference_type; + typedef CSVRow* pointer; + typedef CSVRow& reference; + + CSVIterator(std::istream& str) :m_str(str.good()?&str:NULL) { ++(*this); } + CSVIterator() :m_str(NULL) {} + + // Pre Increment + CSVIterator& operator++() {if (m_str) { if (!((*m_str) >> m_row)){m_str = NULL;}}return *this;} + // Post increment + CSVIterator operator++(int) {CSVIterator tmp(*this);++(*this);return tmp;} + CSVRow const& operator*() const {return m_row;} + CSVRow const* operator->() const {return &m_row;} + + bool operator==(CSVIterator const& rhs) {return ((this == &rhs) || ((this->m_str == NULL) && (rhs.m_str == NULL)));} + bool operator!=(CSVIterator const& rhs) {return !((*this) == rhs);} + private: + std::istream* m_str; + CSVRow m_row; +}; +} +#endif \ No newline at end of file diff --git a/include/grl/flatbuffer/FlatbufferToEigen.hpp b/include/grl/flatbuffer/FlatbufferToEigen.hpp new file mode 100644 index 0000000..ec49413 --- /dev/null +++ b/include/grl/flatbuffer/FlatbufferToEigen.hpp @@ -0,0 +1,109 @@ +#ifndef GRL_FLATBUFFER_TO_EIGEN +#define GRL_FLATBUFFER_TO_EIGEN + +#define FLATBUFFERS_DEBUG_VERIFICATION_FAILURE +// // grl + +#include +#include +#include "camodocal/EigenUtils.h" +#include +#include "grl/vrep/SpaceVecAlg.hpp" + + +namespace grl { + + Eigen::Affine3f MarkerPoseToAffine3f(const Eigen::VectorXd& markerPose){ + + assert(markerPose.size() == 7); + Eigen::Affine3f markerToCameraTransform; + markerToCameraTransform.translation().x() = markerPose[0]; + markerToCameraTransform.translation().y() = markerPose[1]; + markerToCameraTransform.translation().z() = markerPose[2]; + Eigen::Quaterniond q(markerPose[3], markerPose[4], markerPose[5], markerPose[6]); + Eigen::Matrix3d R = q.normalized().toRotationMatrix(); + markerToCameraTransform.matrix().block<3,3>(0,0) = R.cast(); + return markerToCameraTransform; + } + Eigen::VectorXd Affine3fToMarkerPose(const Eigen::Affine3f& markerToCameraTransform){ + Eigen::VectorXd markerPose(7); + + markerPose[0] = markerToCameraTransform.translation().x(); + markerPose[1] = markerToCameraTransform.translation().y(); + markerPose[2] = markerToCameraTransform.translation().z(); + Eigen::Matrix3f R = markerToCameraTransform.matrix().block<3,3>(0,0); + Eigen::Quaterniond q( R.cast()); + markerPose[3] = q.w(); + markerPose[4] = q.x(); + markerPose[5] = q.y(); + markerPose[6] = q.z(); + return markerPose; + } + + Eigen::MatrixXd getPluckerPose(std::vector& Pose ) { + std::size_t pose_size = Pose.size(); + Eigen::MatrixXd PKPose(pose_size,6); + for(int i=0; i getPTransform(Eigen::MatrixXd& PKPose ) { + std::size_t pose_size = PKPose.size(); + std::vector PTPose; + for(int i=0; i invertPose(const std::vector& poseTransformd){ + std::size_t size = poseTransformd.size(); + std::vector inversePose(size); + + for(int i=0; i +#include +#include +#include +#include +#include +#include + + +namespace grl +{ + + +/// Helper function for both KUKA and FusionTrack +grl::flatbuffer::Vector3d toFlatBuffer(const Eigen::Vector3d &pt) +{ + return grl::flatbuffer::Vector3d(pt.x(), pt.y(), pt.z()); +} + +grl::flatbuffer::Quaternion toFlatBuffer(Eigen::Quaterniond q) +{ + return grl::flatbuffer::Quaternion(q.x(), q.y(), q.z(), q.w()); +} + +grl::flatbuffer::Pose toFlatBuffer(Eigen::Affine3d tf) +{ + Eigen::Vector3d pos = tf.translation(); + Eigen::Quaterniond eigenQuat(tf.rotation()); + return grl::flatbuffer::Pose(toFlatBuffer(pos), toFlatBuffer(eigenQuat)); +} + +grl::flatbuffer::Pose toFlatBuffer(Eigen::Affine3f tf) +{ + return toFlatBuffer(tf.cast()); +} + +template +typename T::value_type stringLength(const T &array) +{ + + auto iter = std::find(array.begin(), array.end(), '\0'); + auto len = std::distance(array.begin(), iter); + return len; +} + +flatbuffers::Offset +toFlatBuffer(flatbuffers::FlatBufferBuilder &fbb, + const grl::TimeEvent &timeStamp) +{ + flatbuffers::Offset event_name = fbb.CreateString(const_cast(timeStamp.event_name.begin()), stringLength(timeStamp.event_name)); + /// https://github.com/googlecartographer/cartographer/blob/master/cartographer/common/time.cc + /// convert time to int64 + int64_t local_request_time = cartographer::common::ToUniversal(timeStamp.local_request_time); + flatbuffers::Offset device_clock_id = fbb.CreateString(const_cast(timeStamp.device_clock_id.begin()), stringLength(timeStamp.device_clock_id)); + int64_t device_time = cartographer::common::ToUniversal(timeStamp.device_time); + flatbuffers::Offset local_clock_id = fbb.CreateString(const_cast(timeStamp.local_clock_id.begin()), stringLength(timeStamp.local_clock_id)); + int64_t local_receive_time = cartographer::common::ToUniversal(timeStamp.local_receive_time); + int64_t corrected_local_time = cartographer::common::ToUniversal(timeStamp.corrected_local_time); + int64_t clock_skew = cartographer::common::ToSeconds(timeStamp.clock_skew); + int64_t min_transport_delay = cartographer::common::ToSeconds(timeStamp.min_transport_delay); + return grl::flatbuffer::CreateTimeEvent( + fbb, + event_name, + local_request_time, + device_clock_id, + device_time, + local_clock_id, + local_receive_time, + corrected_local_time, + clock_skew, + min_transport_delay); +} + +} // End of grl namespace + +#endif // GRL_HELPER_TO_FLATBUFFER diff --git a/include/grl/flatbuffer/JointState.fbs b/include/grl/flatbuffer/JointState.fbs index d36cec8..6bc3602 100644 --- a/include/grl/flatbuffer/JointState.fbs +++ b/include/grl/flatbuffer/JointState.fbs @@ -9,4 +9,4 @@ table JointState { torque:[double]; // Newton Meters (N*m) } -root_type JointState; \ No newline at end of file +// root_type JointState; \ No newline at end of file diff --git a/include/grl/flatbuffer/KUKAiiwa.fbs b/include/grl/flatbuffer/KUKAiiwa.fbs index 2b4fd5d..e4a40c0 100644 --- a/include/grl/flatbuffer/KUKAiiwa.fbs +++ b/include/grl/flatbuffer/KUKAiiwa.fbs @@ -240,13 +240,32 @@ table KUKAiiwaMonitorConfiguration { } // Get state data for the arm (things that change often) +// This table isn't used any more, since the main information of this table is the same with table FRIMessageLog table KUKAiiwaMonitorState { + // this is the measured current state of the arm joints + // as seen in monitorState and _FRIMonitoringMessage + // Several values from KukaState should be copied into here + // - KukaState::position + // - KukaState::torque measuredState:JointState; - cartesianFlangePose:Pose; // cartesian pose of the flange relative to the base of the arm - jointStateReal:JointState; // this is the real current state of the arm joints - /// FRI can adjust the java commanded position. "Interpolated" is the original Java commanded position. + // cartesian pose of the flange relative to the base of the arm + cartesianFlangePose:Pose; + + // This is the real estimated state of the arm joints + // after combining data from the arm with sensors + // TODO: Not supported for now + jointStateReal:JointState; + // TODO: verify this description is correct + // FRI can adjust the java commanded position. + // "Interpolated" is the original Java commanded position. + // "ipo" is an interpolated position. Kuka does this strange + // thing where you send a command in Java then use FRI to + // modify or "interpolate", drawing some other path different + // from what the users originally specified. It seems strange, + // confusing, useless. + // KukaState::ipoJointPosition goes in here jointStateInterpolated:JointState; /// The state of the arm as calculated by kuka after @@ -254,20 +273,57 @@ table KUKAiiwaMonitorState { /// and any attachments configured to be present. /// /// Most likely only contains torque. + /// KukaState::ExternalTorque goes here externalState:JointState; + /// KUKA::FRI::EOperationMode operationMode:EOperationMode; + sessionState:ESessionState; + // This is a received cartesian wrench state CartesianWrench:Wrench; } +table FRITimeStamp { + sec: int; + nanosec: int; +} + +// a table that is used as a record of all communication over FRI +// it is not for user configuration or anything else, it should +// simply reflect a history of what was sent and what was received. + +table FRIMessageLog { + + sessionState:ESessionState; + connectionQuality:EConnectionQuality; + controlMode:EControlMode; + messageIdentifier:int; + sequenceCounter:int; + reflectedSequenceCounter:int; + // hardware timestamps, replaced by the device_time in TimeEvent + // sec:int; + // nanosec:int; // one billionth of a second + // MessageMonitorData + measuredJointPosition:[double]; + measuredTorque:[double]; + commandedJointPosition:[double]; + commandedTorque:[double]; + externalTorque:[double]; + // MessageIpoData + jointStateInterpolated:[double]; + + timeStamp:TimeEvent; + } + table KUKAiiwaState { name:string; // The name of the robot to update (identifier used when applicable, doesn't ever change or set the name) destination:string; // where this message is going (URI) source:string; // where this message came from (URI) - timestamp:double; // timestamp in seconds + // timestamp:double; // timestamp in seconds + timeStamp:TimeEvent; setArmControlState:bool = false; // only actually change the arm state when this is true. armControlState:ArmControlState; // Command state and mode of the arm @@ -281,13 +337,16 @@ table KUKAiiwaState { hasMonitorConfig:bool = false; monitorConfig:KUKAiiwaMonitorConfiguration; + FRIMessage:FRIMessageLog; } + // Full message that is sent back and forth // between KUKA iiwa and driver // This is used because they can also be // accumulated and saved to disk as a log // very easily! + table KUKAiiwaStates { states:[KUKAiiwaState]; } diff --git a/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp b/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp new file mode 100644 index 0000000..ff1f313 --- /dev/null +++ b/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp @@ -0,0 +1,902 @@ +#ifndef GRL_READ_DATA_FROM_BINARY +#define GRL_READ_DATA_FROM_BINARY + +#define FLATBUFFERS_DEBUG_VERIFICATION_FAILURE + +// grl +#include "grl/vrep/Vrep.hpp" +#include "grl/vrep/VrepRobotArmDriver.hpp" +#include "grl/vrep/VrepRobotArmJacobian.hpp" +#include "grl/vrep/Eigen.hpp" +#include "grl/vrep/SpaceVecAlg.hpp" + +// +#include "grl/flatbuffer/FlatbufferToEigen.hpp" +#include "grl/flatbuffer/CSVIterator.hpp" + + +// FusionTrack Libraries +// #include + +#include "grl/vector_ostream.hpp" +#include "flatbuffers/flatbuffers.h" +#include "grl/flatbuffer/JointState_generated.h" +#include "grl/flatbuffer/ArmControlState_generated.h" +#include "grl/flatbuffer/KUKAiiwa_generated.h" +#include "grl/flatbuffer/LinkObject_generated.h" +#include "grl/flatbuffer/Time_generated.h" +#include "grl/flatbuffer/flatbuffer.hpp" +#include "grl/flatbuffer/FusionTrack_generated.h" +#include "grl/flatbuffer/LogKUKAiiwaFusionTrack_generated.h" +#include + + +#include + +#include // For printing and file access. +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + + +#include +#include +#include +#include +#include +#include "camodocal/EigenUtils.h" + +#include "grl/time.hpp" + +// SpaceVecAlg +// https://github.com/jrl-umi3218/SpaceVecAlg +#include + + +// RBDyn +// https://github.com/jrl-umi3218/RBDyn +#include +#include +#include +#include +#include +#include + +// mc_rbdyn_urdf +// https://github.com/jrl-umi3218/mc_rbdyn_urdf +#include +#include "kukaiiwaURDF.h" + + + +namespace grl { + + + /// Define some constants. + const double RadtoDegree =1;// 180/3.14159265359; + const double MeterToMM = 1; + /// Define the int64_t vector and matrix, which is used for time data. + typedef Eigen::Matrix VectorXd; + typedef Eigen::Matrix MatrixXd; + + std::vector Time_Labels = { "local_request_time_offset", "local_receive_time_offset", "device_time_offset", "time_Y", "counter"}; + // std::vector PK_Pose_Labels = {"P_X", "P_Y", "P_Z", "Q_X", "Q_Y", "Q_Z", "Q_W"}; + std::vector M_Pos_Joint_Labels = {"M_Pos_J1", "M_Pos_J2", "M_Pos_J3", "M_Pos_J4", "M_Pos_J5", "M_Pos_J6", "M_Pos_J7"}; + std::vector M_Tor_Joint_Labels = {"M_Tor_J1", "M_Tor_J2", "M_Tor_J3", "M_Tor_J4", "M_Tor_J5", "M_Tor_J6", "M_Tor_J7"}; + std::vector C_Pos_Joint_Labels = {"C_Pos_J1", "C_Pos_J2", "C_Pos_J3", "C_Pos_J4", "C_Pos_J5", "C_Pos_J6", "C_Pos_J7"}; + std::vector C_Tor_Joint_Labels = {"C_Tor_J1", "C_Tor_J2", "C_Tor_J3", "C_Tor_J4", "C_Tor_J5", "C_Tor_J6", "C_Tor_J7"}; + std::vector E_Tor_Joint_Labels = {"E_Tor_J1", "E_Tor_J2", "E_Tor_J3", "E_Tor_J4", "E_Tor_J5", "E_Tor_J6", "E_Tor_J7"}; + std::vector IPO_Joint_Labels = {"IPO_J1", "IPO_J2", "IPO_J3", "IPO_J4", "IPO_J5", "IPO_J6", "IPO_J7"}; + std::vector Joint_Labels = {"Joint_1", "Joint_2", "Joint_3", "Joint_4", "Joint_5", "Joint_6", "Joint_7"}; + std::vector Transform_Labels = {"X", "Y", "Z", "Q_W", "Q_X", "Q_Y", "Q_Z"}; + std::vector PK_Pose_Labels = {"X", "Y", "Z", "A", "B", "C"}; + enum TimeType { local_request_time = 0, local_receive_time = 1, device_time = 2, time_Y = 3, counterIdx=4}; + enum Joint_Index { joint_1, joint_2, joint_3, joint_4, joint_5, joint_6, joint_7}; + enum LabelsType { FT_Pose = 0, Joint = 1, Kuka_Pose= 2}; + int timeBaseline_index = TimeType::local_request_time; // Another option is TimeType::local_receive_time + struct kuka_tag {}; + struct fusiontracker_tag {}; + int col_timeEvent=Time_Labels.size(); + int col_Kuka_Joint = Joint_Labels.size(); + const static int jointNum = 7; + int col_Pose = PK_Pose_Labels.size(); + + + /// Get CSV labels + /// @param label indicate the type of the label. + std::vector getLabels( LabelsType label){ + std::vector labels; + labels.insert(labels.end(), Time_Labels.begin(), Time_Labels.end()); + switch(label) { + case FT_Pose: + labels.insert(labels.end(), PK_Pose_Labels.begin(), PK_Pose_Labels.end()); + break; + case Joint: + labels.insert(labels.end(), Joint_Labels.begin(), Joint_Labels.end()); + break; + case Kuka_Pose: + labels.insert(labels.end(), PK_Pose_Labels.begin(), PK_Pose_Labels.end()); + break; + default: + std::cout<<"Only return Time_Labels..."< + bool checkmonotonic( T &time){ + for(int i=1; i + int getStateSize(const T &binaryObjectP) { + auto states = binaryObjectP->states(); + return states->size(); + + } + /// Process the time data to get the time offset based on the starting time point. + /// See https://github.com/facontidavide/PlotJuggler/issues/68 + /// @param timeEventM timeEvent matrix which should have four columns + void regularizeTimeEvent(grl::MatrixXd& timeEventM, + int64_t initial_local_time, + int64_t initial_device_time, + int rowIndex){ + std::cout<< "initial_local_time: " << initial_local_time << std::endl; + std::size_t time_size = timeEventM.rows(); + assert(time_size>0); + timeEventM.col(grl::TimeType::local_request_time) = timeEventM.col(grl::TimeType::local_request_time) - initial_local_time * grl::VectorXd::Ones(time_size); + timeEventM.col(grl::TimeType::local_receive_time) = timeEventM.col(grl::TimeType::local_receive_time) - initial_local_time * grl::VectorXd::Ones(time_size); + timeEventM.col(grl::TimeType::device_time) = timeEventM.col(grl::TimeType::device_time) - initial_device_time * grl::VectorXd::Ones(time_size); + timeEventM.col(grl::TimeType::time_Y) = timeEventM.col(grl::TimeType::time_Y) - timeEventM(rowIndex, grl::TimeType::time_Y) * grl::VectorXd::Ones(time_size); + } + + + /// Get the maker pose based on the markerID. The bad data, which means the frame doesn't have the indicated marker information, has been filtered out. + /// Bad data filtering is provided by this functions(both the marker pose and the corresponding timeEvent is skipped). + /// In the method we get the orientation in Euler-Angles + /// @param logKUKAiiwaFusionTrackP, pointer of the root object for fusiontracker. + /// @param markerID, the indicated marker. + /// @param timeEventM, timeEvent without bad data, which is filled out. + /// @param markerPose, the pose matrix, which is filled out. + /// @return row, the number of valid rows. + int getMarkerPose(const fbs_tk::Root &logKUKAiiwaFusionTrackP, + uint32_t &markerID, + grl::MatrixXd& timeEventM, + Eigen::MatrixXd& markerPose){ + auto states = logKUKAiiwaFusionTrackP->states(); + std::size_t state_size = states->size(); + assert(state_size>0); + // The first columne is counter + int row = 0; + int BadCount = 0; + // Loop through the marker states in flatbuffer binary file and then reach all the parameters we need. + for(int i = 0; iGet(i); + auto FT_Message = kukaiiwaFusionTrackMessage->deviceState_as_FusionTrackMessage(); + auto FT_Frame = FT_Message->frame(); + auto counter = FT_Frame->counter(); + auto Makers = FT_Frame->markers(); + /// In some frames, there are no markers, where we skip that line. Also we need to remove the corresponding line in TimeEvent. + if(Makers!=nullptr) { + auto makerSize = Makers->size(); + for(int markerIndex=0; markerIndexGet(markerIndex); + auto marker_ID = marker->geometryID(); + if(marker_ID == markerID){ + auto timeEvent = kukaiiwaFusionTrackMessage->timeEvent(); + timeEventM(row,TimeType::local_request_time) = timeEvent->local_request_time(); + timeEventM(row,TimeType::local_receive_time) = timeEvent->local_receive_time(); + timeEventM(row,TimeType::device_time) = timeEvent->device_time(); + timeEventM(row,TimeType::time_Y) = timeEventM(row,TimeType::device_time) - timeEventM(row,timeBaseline_index); + + timeEventM(row,TimeType::counterIdx) = counter; + + auto Pose = marker->transform(); + auto FB_r = Pose->position(); + auto FB_q = Pose->orientation(); + // Convert the flatbuffer type to Eigen type + Eigen::Vector3d r(FB_r.x(), FB_r.y(), FB_r.z()); + Eigen::Quaterniond q(FB_q.w(), FB_q.x(), FB_q.y(), FB_q.z()); + Eigen::Matrix3d E = q.normalized().toRotationMatrix(); + Eigen::Vector3d eulerAngleEigen = RadtoDegree*E.eulerAngles(0,1,2); // X Y Z + Eigen::RowVectorXd pose(grl::col_Pose); + pose << r.transpose(), eulerAngleEigen.transpose(); + + markerPose.row(row++) = pose; + + // Once read the specific marker information, skip out of the marker loop. + // It can keep from reading duplicate information. + // Sometimes in the same frame, there exists two markers with the same geometryID + break; + } + } + } else { + // Count the number of bad data. + BadCount++; + } + } + // Resize the matrix. The size of the matrix is initialized by the number of the states, + // because of the bad data, the final size of the matrix should be smaller than the original one. + // + if(row < state_size) { + // For the time_Y axis, all the following value minus the first one, make it start from 0 + int64_t FT_diff = timeEventM(0,TimeType::device_time) - timeEventM(0,timeBaseline_index); + timeEventM.col(TimeType::time_Y) = timeEventM.col(TimeType::time_Y) - FT_diff * grl::VectorXd::Ones(timeEventM.rows()); + markerPose.conservativeResize(row, Eigen::NoChange_t{}); + timeEventM.conservativeResize(row, Eigen::NoChange_t{}); + } + double diff = static_cast(state_size-markerPose.rows()); + std::cout <<"State size: " << state_size <<" markerPose size: " << markerPose.rows() + << " lossing rate " << diff/state_size << " markerID: " << markerID < &logKUKAiiwaFusionTrackP, + uint32_t &markerID, + grl::MatrixXd& timeEventM_FT, + Eigen::MatrixXd& transform, + int startIndex = 0){ + auto states = logKUKAiiwaFusionTrackP->states(); + std::size_t state_size = states->size(); + assert(state_size>0); + // The first columne is counter + int row = 0; + // std::cout <<"State size: " << state_size << " markerPose rows: " << markerPose.rows() << std::endl; + int BadCount = 0; + + for(int stateIdx = startIndex; startIndexGet(stateIdx); + auto FT_Message = kukaiiwaFusionTrackMessage->deviceState_as_FusionTrackMessage(); + auto FT_Frame = FT_Message->frame(); + auto counter = FT_Frame->counter(); + auto Makers = FT_Frame->markers(); + /// In some frames, there are no markers, where we skip that line. Also we need to remove the corresponding line in TimeEvent. + if(Makers!=nullptr) { + auto makerSize = Makers->size(); + for(int markerIndex=0; markerIndexGet(markerIndex); + auto marker_ID = marker->geometryID(); + if(marker_ID == markerID){ + auto timeEvent = kukaiiwaFusionTrackMessage->timeEvent(); + timeEventM_FT(row,TimeType::local_request_time) = timeEvent->local_request_time(); + timeEventM_FT(row,TimeType::local_receive_time) = timeEvent->local_receive_time(); + timeEventM_FT(row,TimeType::device_time) = timeEvent->device_time(); + timeEventM_FT(row,TimeType::time_Y) = timeEventM_FT(row,TimeType::device_time) - timeEventM_FT(row,timeBaseline_index); + timeEventM_FT(row,TimeType::counterIdx) = counter; + auto Pose = marker->transform(); + auto FB_r = Pose->position(); + auto FB_q = Pose->orientation(); + // Convert the flatbuffer type to Eigen type + // Eigen::Vector3d r(FB_r.x(), FB_r.y(), FB_r.z()); + // Eigen::Quaterniond q(FB_q.w(), FB_q.x(), FB_q.y(), FB_q.z()); + transform.row(row++) << FB_r.x(), FB_r.y(), FB_r.z(), FB_q.w(), FB_q.x(), FB_q.y(), FB_q.z(); + break; + } + } + } else { + BadCount++; + } + } + if(row < state_size) { + int64_t FT_diff = timeEventM_FT(0,TimeType::device_time) - timeEventM_FT(0,timeBaseline_index); + timeEventM_FT.col(TimeType::time_Y) = timeEventM_FT.col(TimeType::time_Y) - FT_diff * grl::VectorXd::Ones(timeEventM_FT.rows()); + transform.conservativeResize(row, Eigen::NoChange_t{}); + timeEventM_FT.conservativeResize(row, Eigen::NoChange_t{}); + } + double diff = static_cast(state_size-transform.rows()); + std::cout <<"State size: " << state_size <<" markerPose size: " << transform.rows() + << " lossing rate " << diff/state_size << " markerID: " << markerID < &kukaStatesP, + grl::MatrixXd& timeEventM_Kuka, + Eigen::VectorXd& sequenceCounterVec, + Eigen::VectorXd& reflectedSequenceCounterVec, + Eigen::MatrixXd& measuredJointPosition, + Eigen::MatrixXd& measuredTorque, + Eigen::MatrixXd& commandedJointPosition, + Eigen::MatrixXd& commandedTorque, + Eigen::MatrixXd& externalTorque, + Eigen::MatrixXd& jointStateInterpolated, + int startIndex = 0){ + auto states = kukaStatesP->states(); + std::size_t state_size = states->size(); + Eigen::VectorXf measurdjointPosVec(state_size); + if(startIndex>state_size){ + return -1; + } + + for(int stateIdx = startIndex; stateIdxGet(stateIdx); + auto FRIMessage = KUKAiiwaState->FRIMessage(); + auto timeEvent = FRIMessage->timeStamp(); + timeEventM_Kuka(stateIdx,TimeType::local_request_time) = timeEvent->local_request_time(); + timeEventM_Kuka(stateIdx,TimeType::local_receive_time) = timeEvent->local_receive_time(); + timeEventM_Kuka(stateIdx,TimeType::device_time) = timeEvent->device_time(); + // timeEventM_Kuka(stateIdx,TimeType::time_Y) = timeEventM_Kuka(stateIdx,TimeType::device_time) - timeEventM_Kuka(stateIdx,TimeType::local_receive_time); + timeEventM_Kuka(stateIdx,TimeType::time_Y) = timeEventM_Kuka(stateIdx,TimeType::device_time) - timeEventM_Kuka(stateIdx,timeBaseline_index); + // timeEventM_Kuka(stateIdx,TimeType::counterIdx) = identifier; + sequenceCounterVec(stateIdx) = FRIMessage->sequenceCounter(); + reflectedSequenceCounterVec(stateIdx) = FRIMessage->reflectedSequenceCounter(); + for(int jointIdx=0; jointIdx<7; jointIdx++){ + measuredJointPosition(stateIdx, jointIdx) = FRIMessage->measuredJointPosition()->size()>0 ? FRIMessage->measuredJointPosition()->Get(jointIdx):0; // flatbuffers::Vector * + measuredTorque(stateIdx, jointIdx) = FRIMessage->measuredTorque()->size()>0 ? FRIMessage->measuredTorque()->Get(jointIdx):0; + commandedJointPosition(stateIdx, jointIdx) = FRIMessage->commandedJointPosition()->size()>0 ? FRIMessage->commandedJointPosition()->Get(jointIdx):0; + commandedTorque(stateIdx, jointIdx) = FRIMessage->commandedTorque()->size()>0 ? FRIMessage->commandedTorque()->Get(jointIdx):0; + externalTorque(stateIdx, jointIdx) = FRIMessage->externalTorque()->size()>0 ? FRIMessage->externalTorque()->Get(jointIdx):0; + jointStateInterpolated(stateIdx, jointIdx) = FRIMessage->jointStateInterpolated()->size()>0 ? FRIMessage->jointStateInterpolated()->Get(jointIdx):0; + } + + } + return 1; + } + + /// Write FRI message into a csv file, all the data are read from flatbuffer binary file. + /// @param KUKA_FRI_CSVfilename, the csv file name + void writeFRIMessageToCSV(std::string& KUKA_FRI_CSVfilename, + grl::MatrixXd& timeEventM_Kuka, + Eigen::VectorXd& sequenceCounterVec, + Eigen::VectorXd& reflectedSequenceCounterVec, + Eigen::MatrixXd& measuredJointPosition, + Eigen::MatrixXd& measuredTorque, + Eigen::MatrixXd& commandedJointPosition, + Eigen::MatrixXd& commandedTorque, + Eigen::MatrixXd& externalTorque, + Eigen::MatrixXd& jointStateInterpolated, + int startIndex = 0) { + int kuka_time_size = timeEventM_Kuka.rows(); + assert(kuka_time_size == sequenceCounterVec.size()); + assert(kuka_time_size == reflectedSequenceCounterVec.size()); + assert(kuka_time_size == measuredJointPosition.rows()); + assert(kuka_time_size == measuredTorque.rows()); + assert(kuka_time_size == commandedJointPosition.rows()); + assert(kuka_time_size == commandedTorque.rows()); + assert(kuka_time_size == externalTorque.rows()); + assert(kuka_time_size == jointStateInterpolated.rows()); + auto receive_time = timeEventM_Kuka.col(grl::TimeType::local_receive_time); + // assert(checkmonotonic(receive_time)); + + std::ofstream fs; + // create a name for the file output + fs.open(KUKA_FRI_CSVfilename, std::ofstream::out | std::ofstream::app); + // write the file headers + fs << grl::Time_Labels[grl::TimeType::local_request_time] << "," + << grl::Time_Labels[grl::TimeType::local_receive_time] << "," + << grl::Time_Labels[grl::TimeType::device_time] << "," + << grl::Time_Labels[grl::TimeType::time_Y] << "," + << "SequenceCounter," + << "reflectedSequenceCounter," + << grl::M_Pos_Joint_Labels[grl::Joint_Index::joint_1] << "," + << grl::M_Pos_Joint_Labels[grl::Joint_Index::joint_2] << "," + << grl::M_Pos_Joint_Labels[grl::Joint_Index::joint_3] << "," + << grl::M_Pos_Joint_Labels[grl::Joint_Index::joint_4] << "," + << grl::M_Pos_Joint_Labels[grl::Joint_Index::joint_5] << "," + << grl::M_Pos_Joint_Labels[grl::Joint_Index::joint_6] << "," + << grl::M_Pos_Joint_Labels[grl::Joint_Index::joint_7] << "," + + << grl::M_Tor_Joint_Labels[grl::Joint_Index::joint_1] << "," + << grl::M_Tor_Joint_Labels[grl::Joint_Index::joint_2] << "," + << grl::M_Tor_Joint_Labels[grl::Joint_Index::joint_3] << "," + << grl::M_Tor_Joint_Labels[grl::Joint_Index::joint_4] << "," + << grl::M_Tor_Joint_Labels[grl::Joint_Index::joint_5] << "," + << grl::M_Tor_Joint_Labels[grl::Joint_Index::joint_6] << "," + << grl::M_Tor_Joint_Labels[grl::Joint_Index::joint_7] << "," + + << grl::C_Pos_Joint_Labels[grl::Joint_Index::joint_1] << "," + << grl::C_Pos_Joint_Labels[grl::Joint_Index::joint_2] << "," + << grl::C_Pos_Joint_Labels[grl::Joint_Index::joint_3] << "," + << grl::C_Pos_Joint_Labels[grl::Joint_Index::joint_4] << "," + << grl::C_Pos_Joint_Labels[grl::Joint_Index::joint_5] << "," + << grl::C_Pos_Joint_Labels[grl::Joint_Index::joint_6] << "," + << grl::C_Pos_Joint_Labels[grl::Joint_Index::joint_7] << "," + + << grl::C_Tor_Joint_Labels[grl::Joint_Index::joint_1] << "," + << grl::C_Tor_Joint_Labels[grl::Joint_Index::joint_2] << "," + << grl::C_Tor_Joint_Labels[grl::Joint_Index::joint_3] << "," + << grl::C_Tor_Joint_Labels[grl::Joint_Index::joint_4] << "," + << grl::C_Tor_Joint_Labels[grl::Joint_Index::joint_5] << "," + << grl::C_Tor_Joint_Labels[grl::Joint_Index::joint_6] << "," + << grl::C_Tor_Joint_Labels[grl::Joint_Index::joint_7] << "," + + << grl::E_Tor_Joint_Labels[grl::Joint_Index::joint_1] << "," + << grl::E_Tor_Joint_Labels[grl::Joint_Index::joint_2] << "," + << grl::E_Tor_Joint_Labels[grl::Joint_Index::joint_3] << "," + << grl::E_Tor_Joint_Labels[grl::Joint_Index::joint_4] << "," + << grl::E_Tor_Joint_Labels[grl::Joint_Index::joint_5] << "," + << grl::E_Tor_Joint_Labels[grl::Joint_Index::joint_6] << "," + << grl::E_Tor_Joint_Labels[grl::Joint_Index::joint_7] << "," + + << grl::IPO_Joint_Labels[grl::Joint_Index::joint_1] << "," + << grl::IPO_Joint_Labels[grl::Joint_Index::joint_2] << "," + << grl::IPO_Joint_Labels[grl::Joint_Index::joint_3] << "," + << grl::IPO_Joint_Labels[grl::Joint_Index::joint_4] << "," + << grl::IPO_Joint_Labels[grl::Joint_Index::joint_5] << "," + << grl::IPO_Joint_Labels[grl::Joint_Index::joint_6] << "," + << grl::IPO_Joint_Labels[grl::Joint_Index::joint_7] << std::endl; + + int kuka_index = startIndex; + // int64_t diff = timeEventM_Kuka(startIndex, grl::TimeType::time_Y); + while ( kuka_index < kuka_time_size) + { + + fs << timeEventM_Kuka(kuka_index, grl::TimeType::local_request_time) <<"," + << timeEventM_Kuka(kuka_index, grl::TimeType::local_receive_time) <<"," + << timeEventM_Kuka(kuka_index, grl::TimeType::device_time) <<"," + << timeEventM_Kuka(kuka_index, grl::TimeType::time_Y)<<"," + << sequenceCounterVec(kuka_index) <<"," + << reflectedSequenceCounterVec(kuka_index) <<"," + << measuredJointPosition(kuka_index, grl::Joint_Index::joint_1) <<"," + << measuredJointPosition(kuka_index, grl::Joint_Index::joint_2) <<"," + << measuredJointPosition(kuka_index, grl::Joint_Index::joint_3) <<"," + << measuredJointPosition(kuka_index, grl::Joint_Index::joint_4) <<"," + << measuredJointPosition(kuka_index, grl::Joint_Index::joint_5) <<"," + << measuredJointPosition(kuka_index, grl::Joint_Index::joint_6) <<"," + << measuredJointPosition(kuka_index, grl::Joint_Index::joint_7) <<"," + + << measuredTorque(kuka_index, grl::Joint_Index::joint_1) <<"," + << measuredTorque(kuka_index, grl::Joint_Index::joint_2) <<"," + << measuredTorque(kuka_index, grl::Joint_Index::joint_3) <<"," + << measuredTorque(kuka_index, grl::Joint_Index::joint_4) <<"," + << measuredTorque(kuka_index, grl::Joint_Index::joint_5) <<"," + << measuredTorque(kuka_index, grl::Joint_Index::joint_6) <<"," + << measuredTorque(kuka_index, grl::Joint_Index::joint_7) <<"," + + << commandedJointPosition(kuka_index, grl::Joint_Index::joint_1) <<"," + << commandedJointPosition(kuka_index, grl::Joint_Index::joint_2) <<"," + << commandedJointPosition(kuka_index, grl::Joint_Index::joint_3) <<"," + << commandedJointPosition(kuka_index, grl::Joint_Index::joint_4) <<"," + << commandedJointPosition(kuka_index, grl::Joint_Index::joint_5) <<"," + << commandedJointPosition(kuka_index, grl::Joint_Index::joint_6) <<"," + << commandedJointPosition(kuka_index, grl::Joint_Index::joint_7) <<"," + + << commandedTorque(kuka_index, grl::Joint_Index::joint_1) <<"," + << commandedTorque(kuka_index, grl::Joint_Index::joint_2) <<"," + << commandedTorque(kuka_index, grl::Joint_Index::joint_3) <<"," + << commandedTorque(kuka_index, grl::Joint_Index::joint_4) <<"," + << commandedTorque(kuka_index, grl::Joint_Index::joint_5) <<"," + << commandedTorque(kuka_index, grl::Joint_Index::joint_6) <<"," + << commandedTorque(kuka_index, grl::Joint_Index::joint_7) <<"," + + << externalTorque(kuka_index, grl::Joint_Index::joint_1) <<"," + << externalTorque(kuka_index, grl::Joint_Index::joint_2) <<"," + << externalTorque(kuka_index, grl::Joint_Index::joint_3) <<"," + << externalTorque(kuka_index, grl::Joint_Index::joint_4) <<"," + << externalTorque(kuka_index, grl::Joint_Index::joint_5) <<"," + << externalTorque(kuka_index, grl::Joint_Index::joint_6) <<"," + << externalTorque(kuka_index, grl::Joint_Index::joint_7) <<"," + + << jointStateInterpolated(kuka_index, grl::Joint_Index::joint_1) <<"," + << jointStateInterpolated(kuka_index, grl::Joint_Index::joint_2) <<"," + << jointStateInterpolated(kuka_index, grl::Joint_Index::joint_3) <<"," + << jointStateInterpolated(kuka_index, grl::Joint_Index::joint_4) <<"," + << jointStateInterpolated(kuka_index, grl::Joint_Index::joint_5) <<"," + << jointStateInterpolated(kuka_index, grl::Joint_Index::joint_6) <<"," + << jointStateInterpolated(kuka_index, grl::Joint_Index::joint_7) << std::endl; + kuka_index++; + } + + std::cout << "kuka_time_size: " << kuka_time_size << " kuka_index: " << kuka_index << std::endl; + fs.close(); + } + + /// Write the data to CSV file + /// @param CSV_FileName, the file name. + /// @param labels, the labels for the data to write, which should be the first row of the csv file + /// @param timeM, by default we write all the data combined with the timestamp, whose type is grl::MatrixXd (int64_t). + /// @param dataM, the data to write, whose type is Eigen::MatrixXd + /// @param rowIndex, In order to make the data from two devices match each other, we just take down the data from the specific row. + template + void writeMatrixToCSV(const std::string& CSV_FileName, std::vector &labels, T1& timeM, T2& dataM, int rowIndex=0){ + std::size_t labels_size = labels.size(); + std::size_t cols_size = timeM.cols() + dataM.cols(); + std::size_t time_rows_size = timeM.rows(); + std::size_t data_rows_size = dataM.rows(); + assert(labels_size == cols_size && time_rows_size>0 && cols_size>0 && time_rows_size==data_rows_size); + auto time = timeM.col(TimeType::local_receive_time); + // assert(checkmonotonic(time)); + // create an ofstream for the file output (see the link on streams for more info) + std::ofstream fs; + // create a name for the file output + fs.open(CSV_FileName, std::ofstream::out | std::ofstream::app); + // write the file labels; + for(int col=0; col0) { + device_time_step = device_timeV(i) - device_timeV(i-1); + receive_time_step = local_receive_timeV(i) - local_receive_timeV(i-1); + } + // write the data to the output file + fs << local_request_timeV(i) <<"," + << local_receive_timeV(i) << "," + << device_timeV(i) <<"," + << time_YV(i) <<"," + << receive_request(i) << "," + << device_time_step << "," + << receive_time_step << "," + << counter(i) << std::endl; //D + } + fs.close(); + } + + + /// Write the tracker and kuka messages inito one single csv file. + /// @param FTKUKA_TimeEvent_CSVfilename, the csv file name + /// @param timeEventM_FT the tracker time event matrix + /// @param timeEventM_Kuka the kuka time event matrix + /// @param jointAngles, joint angles matrix, collected from real robot + /// @param markerPose, marker pose from tracker + /// @param FT_index, kuka_index, since we skip the first rows to make two devices time consistency, here these two indexs are to indicate from where to read or write data. + + void writeFTKUKAMessageToCSV(std::string& FTKUKA_TimeEvent_CSVfilename, + grl::MatrixXd& timeEventM_FT, + grl::MatrixXd& timeEventM_Kuka, + Eigen::MatrixXd& jointAngles, + Eigen::MatrixXd& markerPose, + int FT_index=0, + int kuka_index=0) { + + int FT_time_size = timeEventM_FT.rows(); + int kuka_time_size = timeEventM_Kuka.rows(); + grl::VectorXd FT_local_request_time = timeEventM_FT.col(local_request_time); + grl::VectorXd FT_local_receive_time = timeEventM_FT.col(local_receive_time); + grl::VectorXd FT_device_time = timeEventM_FT.col(device_time); + // assert(checkmonotonic(FT_local_receive_time)); // Hit the asserion, should check the time data from FT_index, not from 0 + + grl::VectorXd kuka_local_request_time = timeEventM_Kuka.col(local_request_time); + grl::VectorXd kuka_local_receive_time = timeEventM_Kuka.col(local_receive_time); + grl::VectorXd kuka_device_time = timeEventM_Kuka.col(device_time); + // assert(checkmonotonic(kuka_local_receive_time)); + // Use the receive time as the baseline. + // int64_t kuka_diff = kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index); + // int64_t FT_diff = FT_device_time(FT_index) - FT_local_receive_time(FT_index); + // Use the request time as the baseline + int64_t kuka_diff = kuka_device_time(kuka_index) - kuka_local_request_time(kuka_index); + int64_t FT_diff = FT_device_time(FT_index) - FT_local_request_time(FT_index); + + grl::VectorXd Y_kuka = kuka_device_time - kuka_local_request_time - grl::VectorXd::Ones(kuka_time_size)*kuka_diff; + grl::VectorXd Y_FT = FT_device_time - FT_local_request_time - grl::VectorXd::Ones(FT_time_size)*FT_diff; + + + + std::ofstream fs; + // create a name for the file output + fs.open( FTKUKA_TimeEvent_CSVfilename, std::ofstream::out | std::ofstream::app); + // write the file headers + fs << "local_request_time," + << "local_receive_time," + << "FT_device_time_offset," + << "KUKA_device_time_offset," + << "Y_FT," + << "Y_kuka," + << "FT_X," + << "FT_Y," + << "FT_Z," + << "FT_A" + << "FT_B," + << "FT_C," + << "K_Joint1," + << "K_Joint2," + << "K_Joint3," + << "K_Joint4," + << "K_Joint5," + << "K_Joint6," + << "K_Joint7" + << std::endl; + std::cout << "Start to write ... "<< std::endl <<"FT_index: " << FT_index << " kuka_index: " << kuka_index << std::endl; + + while ( kuka_index < kuka_time_size && FT_index < FT_time_size ) + { + // If the row value is the kuka time, then the FT cells should be left empty. + // Use the receive time as the baseline + // if ( kuka_local_receive_time(kuka_index) < FT_local_receive_time(FT_index) ){ + if ( kuka_local_request_time(kuka_index) < FT_local_request_time(FT_index) ){ + // write the data to the output file + auto jointrow = jointAngles.row(kuka_index); + fs << kuka_local_request_time(kuka_index) <<"," + << kuka_local_receive_time(kuka_index) << "," + << "," + << kuka_device_time(kuka_index) <<"," + << "," + << Y_kuka(kuka_index) << "," + << ",,,,,," + << jointrow[0] << "," + << jointrow[1] << "," + << jointrow[2] << "," + << jointrow[3] << "," + << jointrow[4] << "," + << jointrow[5] << "," + << jointrow[6] << std::endl; + kuka_index++; + } else if( kuka_local_request_time(kuka_index) > FT_local_request_time(FT_index)) { + auto matrixrow = markerPose.row(FT_index); + // If the row value is the FT time, then the kuka cells should be left blank. + fs << FT_local_request_time(FT_index) << "," + << FT_local_receive_time(FT_index) <<"," + << FT_device_time(FT_index) << "," + << "," + << Y_FT(FT_index) << "," + << "," + << matrixrow[0] << "," + << matrixrow[1] << "," + << matrixrow[2] << "," + << matrixrow[3] << "," + << matrixrow[4] << "," + << matrixrow[5] << std::endl; + FT_index++; + } else { + // In case the time is extactly equivent with each other. + fs << FT_local_request_time(FT_index) << "," + << FT_local_receive_time(FT_index) <<"," + // << kuka_local_request_time(kuka_index) << "," + << FT_device_time(FT_index) << "," + << kuka_device_time(kuka_index) <<"," + << Y_FT(FT_index)<< "," + << Y_kuka(kuka_index) + << std::endl; + FT_index++; + kuka_index++; + } + } + std::cout << "FT_index: " << FT_index << " kuka_index: " << kuka_index << " Sum: " << FT_index+kuka_index << std::endl; + fs.close(); +} + + /// Based on the RBDy and URDF model, get the cartesian pose of the end effector. + /// BE CAREFUL THAT THE URDF MODEL HAS BEEN MODIFIED, THE MARKER LINK HAS BEEN ADDED. + /// + /// This is gotten from VREP, the oritation of the marker dummy is identical with the flange ('Fiducial#22' and 'RobotFlangeTip'). + /// SEE kukaiiwaURDF.h + /// @param jointAngles, the joint angles matrix + /// @param markerPose, if true put the pose of endeffector into the marker space, otherwise in robot frame. + /// @return poseEE, contain the translation and the Euler angle. + std::vector getPoseEE(Eigen::MatrixXd& jointAngles, bool markerPose = false){ + using namespace Eigen; + using namespace sva; + using namespace rbd; + + namespace cst = boost::math::constants; + auto strRobot = mc_rbdyn_urdf::rbdyn_from_urdf(XYZSarmUrdf); + rbd::MultiBody mb = strRobot.mb; + rbd::MultiBodyConfig mbc(mb); + rbd::MultiBodyGraph mbg(strRobot.mbg); + std::size_t nrJoints = mb.nrJoints(); + std::size_t nrBodies = strRobot.mb.nrBodies(); + std::vector jointNames; + + std::size_t row_size = jointAngles.rows(); + std::size_t body_size = mbc.bodyPosW.size(); + /// The translation and Euler angles in world coordinate. + /// + std::vector EEpose; + + for(int rowIdx=0; rowIdx 0) { + mbc.q[jointIndex][0] = oneStateJointPosition[jointIdx]; + jointIdx++; + } + } + rbd::forwardKinematics(mb, mbc); + // Pose of the end effector relative to robot base frame. + if(markerPose){ + EEpose.push_back(mbc.bodyPosW[body_size-1]); + } else{ + EEpose.push_back(mbc.bodyPosW[body_size-2]); + } + } + return std::move(EEpose); + } + + mc_rbdyn_urdf::URDFParserResult getURDFModel(std::string filename){ + if(!boost::filesystem::exists(filename)){ + std::cerr << filename << " doesn't exist..." << std::endl; + } + namespace cst = boost::math::constants; + std::string urdfmodel = readFile(filename); + auto strRobot = mc_rbdyn_urdf::rbdyn_from_urdf(urdfmodel); + return std::move(strRobot); + } + + +int getRowsNumber(std::string filename){ + + std::ifstream file(filename); + std::size_t row_size = 0; + + if (!file.is_open()) { + std::cout << "failed to open " << filename << '\n'; + } else { + grl::CSVIterator loop(file); + + for(grl::CSVIterator loop(file); loop != grl::CSVIterator(); ++loop) + { + row_size++; + } + } + return row_size; + +} + + /// Get the joint angles at specific time point (index) + /// @return jointPosition, Eigen vector which contains joint position of the seven joints. + int64_t getJointAnglesFromCSV(std::string filename, Eigen::VectorXf &jointPosition, int rowIdx, bool commanddata){ + + + // int rowNum = getRowsNumber(filename); + std::ifstream file(filename); + std::size_t joint_size = 7; + // Eigen::VectorXf jointPosition(joint_size); + if (!file.is_open()) { + std::cout << "failed to open " << filename << '\n'; + } else { + grl::CSVIterator loop(file); + + int row = 0; + + for(grl::CSVIterator loop(file); loop != grl::CSVIterator(); ++loop) + { + if(row == rowIdx){ + + for(int joint_index = 0; joint_index((*loop)[joint_index+5]); + } + if(commanddata){ + return boost::lexical_cast((*loop)[0]); + } + return boost::lexical_cast((*loop)[1]); + + } + row++; + + } + } + return -1; + + } + + /// Get the joint angles at specific time point (index) + + /// @return jointPosition, Eigen vector which contains joint position of the seven joints. + Eigen::VectorXf getMarkerPoseFromCSV(std::string filename, int rowIdx){ + + std::ifstream file(filename); + std::size_t size = 6; + Eigen::VectorXf markerpose(size); + if (!file.is_open()) { + std::cout << "failed to open " << filename << '\n'; + } else { + grl::CSVIterator loop(file); + + int row = 0; + + for(grl::CSVIterator loop(file); loop != grl::CSVIterator(); ++loop) + { + + if(row == rowIdx){ + for(int index = 0; index jointNames; + // std::cout<<"Joint Size: "<< nrJoints << std::endl; + + } + + + + + +} +#endif \ No newline at end of file diff --git a/include/grl/flatbuffer/Robone_KukaLBRiiwa.urdf b/include/grl/flatbuffer/Robone_KukaLBRiiwa.urdf new file mode 100644 index 0000000..233d06e --- /dev/null +++ b/include/grl/flatbuffer/Robone_KukaLBRiiwa.urdf @@ -0,0 +1,426 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + /iiwa + + + + + Gazebo/Grey + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Grey + 0.2 + 0.2 + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + \ No newline at end of file diff --git a/include/grl/flatbuffer/flatbuffer.hpp b/include/grl/flatbuffer/flatbuffer.hpp index a652c76..b0180f2 100644 --- a/include/grl/flatbuffer/flatbuffer.hpp +++ b/include/grl/flatbuffer/flatbuffer.hpp @@ -1,171 +1,171 @@ #ifndef GRL_FLATBUFFER_HPP #define GRL_FLATBUFFER_HPP +#include #include #include +// boost::filesystem +#include +#include +#include namespace grl { -// loads a json flatbuffer from a file -bool LoadJSONFlatbuffer(flatbuffers::Parser& parser, std::string schemaPath, std::string jsonPath, std::string include_directory, bool binary = false) -{ - // source: https://github.com/google/flatbuffers/blob/master/samples/sample_text.cpp - - // load FlatBuffer schema (.fbs) and JSON from disk - std::string schemafile; - std::string jsonfile; - bool ok = flatbuffers::LoadFile(schemaPath.c_str(), binary, &schemafile) && - flatbuffers::LoadFile(jsonPath.c_str(), binary, &jsonfile); - if (!ok) { - printf("couldn't load files!\n"); - return ok; - } - - // parse schema first, so we can use it to parse the data after - const char *include_directories[] = { include_directory.c_str(), "flatbuffer", nullptr }; - ok = parser.Parse(schemafile.c_str(), include_directories) && - parser.Parse(jsonfile.c_str(), include_directories); - assert(ok); - return ok; -} - -/// @brief Loads a flatbuffers schema file from disk into a flatbuffers parser. -/// -/// @param include_paths JSON ONLY. This is the list of paths describing where to find the -/// flatbuffers schema files (.fbs) required to write the json file. -/// In particular, LogKUKAiiwaFusionTrack.fbs is requred. If the empty -/// string or an empty list is specified, the current working directory -/// will be checked for LogKUKAiiwaFusionTrack.fbs and the other files -/// it depends on. -/// @param read_binary_schema JSON only. The LogKUKAiiwaFusionTrack.fbs and other schemea files -/// can be saved in ascii plaintext format (the default) and in binary format. -/// Set this option to true if you expect to load the schema files in binary format. -/// @param write_binary_schema If "binary_stream" is false data is written using ifstream's -/// text mode, otherwise data is written with no transcoding. -bool ParseSchemaFile( - flatbuffers::Parser& parser, - std::string fbs_filename, - std::vector includePaths = std::vector(), - bool read_binary_schema=false) -{ - - bool loadfbsfile_ok = true; - std::string schemafile; - if(includePaths.empty()) + // loads a json flatbuffer from a file + bool LoadJSONFlatbuffer(flatbuffers::Parser& parser, std::string schemaPath, std::string jsonPath, std::string include_directory, bool binary = false) { + // source: https://github.com/google/flatbuffers/blob/master/samples/sample_text.cpp + // load FlatBuffer schema (.fbs) and JSON from disk + std::string schemafile; + std::string jsonfile; + bool ok = flatbuffers::LoadFile(schemaPath.c_str(), binary, &schemafile) && + flatbuffers::LoadFile(jsonPath.c_str(), binary, &jsonfile); + if (!ok) { + printf("couldn't load files!\n"); + return false; + } - std::string current_working_dir; - char buff[2048]; - /// Get the current working directory - getcwd(buff, 2048); - current_working_dir = std::string(buff); - includePaths.push_back(current_working_dir); + // parse schema first, so we can use it to parse the data after + const char *include_directories[] = { include_directory.c_str(), "flatbuffer", nullptr }; + ok = parser.Parse(schemafile.c_str(), include_directories) && + parser.Parse(jsonfile.c_str(), include_directories); + assert(ok &&"LoadJSONFlatbuffer"); + return ok; } - std::string fbs_fullpath; - // check if the user passed a full path to the fbs file - if(flatbuffers::FileExists(fbs_filename.c_str())){ - fbs_fullpath = fbs_filename; - } - else + /// @brief Loads a flatbuffers schema file from disk into a flatbuffers parser. + /// + /// @param include_paths JSON ONLY. This is the list of paths describing where to find the + /// flatbuffers schema files (.fbs) required to write the json file. + /// In particular, LogKUKAiiwaFusionTrack.fbs is requred. If the empty + /// string or an empty list is specified, the current working directory + /// will be checked for LogKUKAiiwaFusionTrack.fbs and the other files + /// it depends on. + /// @param read_binary_schema JSON only. The LogKUKAiiwaFusionTrack.fbs and other schemea files + /// can be saved in ascii plaintext format (the default) and in binary format. + /// Set this option to true if you expect to load the schema files in binary format. + /// @param write_binary_schema If "binary_stream" is false data is written using ifstream's + /// text mode, otherwise data is written with no transcoding. + bool ParseSchemaFile( + flatbuffers::Parser& parser, + std::string fbs_filename, + std::vector includePaths = std::vector(), + bool read_binary_schema=false) { - // a full path wasn't passed, so check all the include paths - for(auto includePath : includePaths) + bool loadfbsfile_ok = true; + std::string schemafile; + if(includePaths.empty()) { - std::string fbs_trypath = flatbuffers::ConCatPathFileName(includePath, fbs_filename); - if(flatbuffers::FileExists(fbs_trypath.c_str())) - { - fbs_fullpath = fbs_trypath; - // we found it! no need to keep looping - break; - } + + std::string current_working_dir = boost::filesystem::current_path().string(); + //std::cout << "The current working dir: " << current_working_dir << std::endl; + includePaths.push_back(current_working_dir); } - } - // if it is empty none of the files actually existed, return false - if(fbs_fullpath.empty()) return false; - if(!flatbuffers::LoadFile(fbs_filename.c_str(), read_binary_schema, &schemafile)) - { - // something is wrong - return false; - } + std::string fbs_fullpath; + // check if the user passed a full path to the fbs file + if(flatbuffers::FileExists(fbs_filename.c_str())){ + fbs_fullpath = fbs_filename; + } + else + { + //std::cout << "a full path wasn't passed, so check all the include paths" << std::endl; + for(auto includePath : includePaths) + { + std::string fbs_trypath = flatbuffers::ConCatPathFileName(includePath, fbs_filename); + if(flatbuffers::FileExists(fbs_trypath.c_str())) + { + fbs_fullpath = fbs_trypath; + // we found it! no need to keep looping + break; + } + } + } - // parse fbs schema, so we can use it to parse the data after - // create a list of char* pointers so we can call Parse - std::vector include_directories; - for(int i = 0; i < includePaths.size(); i++){ - include_directories.push_back(includePaths[i].c_str()); - } - include_directories.push_back(nullptr); - - return parser.Parse(schemafile.c_str(), &include_directories[0]); -} - - -/// -/// Save data flatbuffers::DetachedBuffer db into a file binary_file_path, -/// returning true if successful, false otherwise. Prefer reading and -/// writing binary files over json files, because they are much more efficient. -/// -/// @param db flatbuffers detached buffer object to write to disk -/// @param binary_file_path full file path to write the binary log file. -/// Empty string indicates this file should not be saved. -/// Please prefer saving binary files and use the file extension .flik. -/// @param fbs_file_path JSON Only. The file name of the flatbuffers schema definition file (.fbs). -/// If this is empty the JSON file will not be saved. -/// @param json_file_path full file path to write the json file, -/// Only write this file for debugging purposes, it is very expensive. -/// The default empty string indicates this file should not be saved. -/// Please use the file extension .json if saving this file. -/// @param include_paths JSON ONLY. This is the list of paths describing where to find the -/// flatbuffers schema files (.fbs) required to write the json file. -/// In particular, LogKUKAiiwaFusionTrack.fbs is requred. If the empty -/// string or an empty list is specified, the current working directory -/// will be checked for LogKUKAiiwaFusionTrack.fbs and the other files -/// it depends on. -/// @param read_binary_schema JSON only. The LogKUKAiiwaFusionTrack.fbs and other schemea files -/// can be saved in ascii plaintext format (the default) and in binary format. -/// Set this option to true if you expect to load the schema files in binary format. -/// @param write_binary_schema If "binary_stream" is false data is written using ifstream's -/// text mode, otherwise data is written with no transcoding. -bool SaveFlatBufferFile( - const uint8_t* buffer, - std::size_t size, - std::string binary_file_path, - std::string fbs_filename = std::string(), - std::string json_file_path = std::string(), - std::vector includePaths = std::vector(), - bool read_binary_schema=false, - bool write_binary_stream=true) -{ - bool ok = true; - ///////////////////////////////////// - /// Saving BINARY version of file /// - ///////////////////////////////////// - if(!binary_file_path.empty()) - { - ok = ok && flatbuffers::SaveFile(binary_file_path.c_str(), reinterpret_cast(buffer), size, write_binary_stream); + // if it is empty none of the files actually existed, return false + if(fbs_fullpath.empty()) return false; + if(!flatbuffers::LoadFile(fbs_filename.c_str(), read_binary_schema, &schemafile)) + { + // something is wrong + return false; + } + + // parse fbs schema, so we can use it to parse the data after + // create a list of char* pointers so we can call Parse + std::vector include_directories; + for(int i = 0; i < includePaths.size(); i++){ + include_directories.push_back(includePaths[i].c_str()); + } + include_directories.push_back(nullptr); + return parser.Parse(schemafile.c_str(), &include_directories[0]); } - //////////////////////////////////////////// - /// Load fbs file and generate json file /// - //////////////////////////////////////////// - if(!json_file_path.empty() && !fbs_filename.empty()) + /// + /// Save data flatbuffers::DetachedBuffer db into a file binary_file_path, + /// returning true if successful, false otherwise. Prefer reading and + /// writing binary files over json files, because they are much more efficient. + /// + /// @param db flatbuffers detached buffer object to write to disk + /// @param binary_file_path full file path to write the binary log file. + /// Empty string indicates this file should not be saved. + /// Please prefer saving binary files and use the file extension .flik. + /// @param fbs_file_path JSON Only. The file name of the flatbuffers schema definition file (.fbs). + /// If this is empty the JSON file will not be saved. + /// @param json_file_path full file path to write the json file, + /// Only write this file for debugging purposes, it is very expensive. + /// The default empty string indicates this file should not be saved. + /// Please use the file extension .json if saving this file. + /// @param include_paths JSON ONLY. This is the list of paths describing where to find the + /// flatbuffers schema files (.fbs) required to write the json file. + /// In particular, LogKUKAiiwaFusionTrack.fbs is requred. If the empty + /// string or an empty list is specified, the current working directory + /// will be checked for LogKUKAiiwaFusionTrack.fbs and the other files + /// it depends on. + /// @param read_binary_schema JSON only. The LogKUKAiiwaFusionTrack.fbs and other schemea files + /// can be saved in ascii plaintext format (the default) and in binary format. + /// Set this option to true if you expect to load the schema files in binary format. + /// @param write_binary_schema If "binary_stream" is false data is written using ifstream's + /// text mode, otherwise data is written with no transcoding. + bool SaveFlatBufferFile( + const uint8_t* buffer, + std::size_t size, + std::string binary_file_path, + std::string fbs_filename = std::string(), + std::string json_file_path = std::string(), + std::vector includePaths = std::vector(), + bool read_binary_schema=false, + bool write_binary_stream=true) { - flatbuffers::Parser parser; - - ok = ok && ParseSchemaFile(parser, fbs_filename, includePaths, read_binary_schema); - std::string jsongen; - // now generate text from the flatbuffer binary - ok = ok && GenerateText(parser, buffer, &jsongen); - // Write the data get from flatbuffer binary to json file on disk. - std::ofstream out(json_file_path); - out << jsongen.c_str(); - out.close(); - } - return ok; -} + bool ok = true; + ///////////////////////////////////// + /// Saving BINARY version of file /// + ///////////////////////////////////// + if(!binary_file_path.empty()) + { + ok = ok && flatbuffers::SaveFile(binary_file_path.c_str(), reinterpret_cast(buffer), size, write_binary_stream); + } + + //////////////////////////////////////////// + /// Load fbs file and generate json file /// + //////////////////////////////////////////// + if(!json_file_path.empty() && !fbs_filename.empty()) + { + flatbuffers::Parser parser; + + ok = ok && ParseSchemaFile(parser, fbs_filename, includePaths, read_binary_schema); + std::string jsongen; + // now generate text from the flatbuffer binary + + ok = ok && GenerateText(parser, buffer, &jsongen); + // Write the data get from flatbuffer binary to json file on disk. + + std::ofstream out(json_file_path); + out << jsongen.c_str(); + out.close(); + } + return ok; + } } // namespace grl #endif // GRL_FLATBUFFER_HPP \ No newline at end of file diff --git a/include/grl/flatbuffer/kukaiiwaURDF.h b/include/grl/flatbuffer/kukaiiwaURDF.h new file mode 100644 index 0000000..6f23b6f --- /dev/null +++ b/include/grl/flatbuffer/kukaiiwaURDF.h @@ -0,0 +1,508 @@ +/* Copyright 2015-2017 CNRS-UM LIRMM, CNRS-AIST JRL + * + * This file is part of mc_rbdyn_urdf. + * + * mc_rbdyn_urdf is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * mc_rbdyn_urdf is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with mc_rbdyn_urdf. If not, see . + */ +/** + * THIS IS A TEMPORAL FILE, AFTER FINISHING THE TEST, REMOVE IT. + * @CHUNTING + */ +#pragma once + +#include +#include +#include + +#include "RBDyn/FK.h" + +// R"( represents a raw string literal in which the escape characters (like \n \t or \" ) of C++ are not processed. +// A raw string literal starts with R"( and ends in )" +// See https://solarianprogrammer.com/2011/10/16/cpp-11-raw-strings-literals-tutorial/ +std::string XYZSarmUrdf( +R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + /iiwa + + + + + Gazebo/Grey + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Grey + 0.2 + 0.2 + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + +)"); + +std::string readFile(const std::string &fileName) +{ + std::ifstream ifs(fileName.c_str(), std::ios::in | std::ios::binary | std::ios::ate); + + std::ifstream::pos_type fileSize = ifs.tellg(); + ifs.seekg(0, ios::beg); + + std::vector bytes(fileSize); + ifs.read(&bytes[0], fileSize); + + return std::string(&bytes[0], fileSize); +} + +namespace mc_rbdyn_urdf +{ +bool operator==( const Geometry::Mesh& m1, const Geometry::Mesh& m2) +{ + return m1.scale == m2.scale && m1.filename == m2.filename; +} + +bool operator==( const Geometry::Box& b1, const Geometry::Box& b2) +{ + return b1.size == b2.size; +} + +bool operator==( const Geometry::Sphere& b1, const Geometry::Sphere& b2) +{ + return b1.radius == b2.radius; +} + +bool operator==( const Geometry::Cylinder& b1, const Geometry::Cylinder& b2) +{ + return b1.radius == b2.radius && b1.length == b2.length; +} + +bool operator==(const Geometry& g1, const Geometry& g2) +{ + return g1.type == g2.type && g1.data == g2.data; +} +bool operator==(const Visual& v1, const Visual& v2) { + return v1.name == v2.name && v1.origin == v2.origin && v1.geometry == v2.geometry; +} +} /* mc_rbdyn_urdf */ + + +const double TOL = 1e-6; \ No newline at end of file diff --git a/include/grl/kuka/Kuka.hpp b/include/grl/kuka/Kuka.hpp index ff837c4..4d6bcf7 100644 --- a/include/grl/kuka/Kuka.hpp +++ b/include/grl/kuka/Kuka.hpp @@ -9,11 +9,17 @@ #include #include #include +#include #include "grl/flatbuffer/KUKAiiwa_generated.h" #include "grl/tags.hpp" #include "grl/exception.hpp" +// used for time synchronization +#include "grl/TimeEvent.hpp" + +#include +#include namespace KUKA { namespace LBRState { @@ -39,20 +45,32 @@ namespace arm { /// do not depend on this struct directly. /// @todo commandedPosition and commandedPosition_goal are used a bit /// ambiguously, figure out the difference and clean it up. +/// @TODO(ahundt) add support for a vector of Transformation data @see FRIMessages.pb.h, +/// but store as quaternon + vector, not rotation matrix struct KukaState { - typedef boost::container::static_vector - joint_state; + typedef boost::container::static_vector joint_state; + // cartesian state entries consist of a vector x,y,z and a quaternion [x, y, z, w] typedef boost::container::static_vector cartesian_state; - typedef std::chrono::time_point - time_point_type; + /// Class std::chrono::high_resolution_clock represents the clock with the smallest tick period provided by the implementation. + typedef std::chrono::time_point time_point_type; + /// measured position, identified by revolute_joint_angle_open_chain_state_tag + /// @see grl::revolute_joint_angle_open_chain_state_tag in grl/tags.hpp, which identifies this data joint_state position; + /// measured torque, identified by revolute_joint_torque_open_chain_state_tag + /// @see grl::revolute_joint_torque_open_chain_state_tag in grl/tags.hpp, which identifies this data joint_state torque; + /// torque applied to the robot from an outside source, + /// @see grl::revolute_joint_torque_open_chain_state_tag in grl/tags.hpp, which identifies this data joint_state externalTorque; + /// measured external force, only available in sunrise OS version 1.9 cartesian_state externalForce; + + /// commanded joint angles + /// @see grl::revolute_joint_angle_open_chain_command_tag in grl/tags.hpp, which identifies this data joint_state commandedPosition; + /// cartesian_wrench_command_tag cartesian_state commandedCartesianWrenchFeedForward; - cartesian_state wrenchJava; joint_state commandedTorque; joint_state ipoJointPosition; @@ -64,17 +82,11 @@ struct KukaState { // which needed to be reimplemented due to licensing restrictions // in the corresponding C++ code flatbuffer::ESessionState sessionState; // KUKA::FRI::ESessionState - flatbuffer::EConnectionQuality - connectionQuality; // KUKA::FRI::EConnectionQuality + flatbuffer::EConnectionQuality connectionQuality; // KUKA::FRI::EConnectionQuality flatbuffer::ESafetyState safetyState; // KUKA::FRI::ESafetyState flatbuffer::EOperationMode operationMode; // KUKA::FRI::EOperationMode flatbuffer::EDriveState driveState; // KUKA::FRI::EDriveState - // The point in time associated with the current measured - // state of the arm (position, torque, etc.). When commanding - // the arm use commanded_goal_timestamp. - time_point_type timestamp; - ///////////////////////////////////////////////////////////////////////////////////////////// // members below here define the driver state and are not part of the FRI arm // message format @@ -98,6 +110,16 @@ struct KukaState { /// velocity limits the arm stops immediately with an error. joint_state velocity_limits; + /// Time event records the hardware time, + /// the local computer time before receiving data + /// and the local computer time after receiving data + /// This makes it possible to more accurately synchronize + /// time between multiple hardware devices. + + // The point in time associated with the current measured + // state of the arm (position, torque, etc.). When commanding + // the arm use commanded_goal_timestamp. + grl::TimeEvent time_event_stamp; void clear() { position.clear(); @@ -120,8 +142,10 @@ struct KukaState { } }; +/// constexpr objects are const and are initiallized with values known during compilation constexpr auto KUKA_LBR_IIWA_14_R820 = "KUKA_LBR_IIWA_14_R820"; -constexpr auto KUKA_LBR_IIWA_7_R800 = "KUKA_LBR_IIWA_7_R800"; +/// constexpr auto KUKA_LBR_IIWA_7_R8, identified by00 = "KUKA_LBR_IIWA_7_R800"; +constexpr auto KUKA_LBR_IIWA_7_R8 = "KUKA_LBR_IIWA_7_R800"; /// @brief copy vector of joint velocity limits in radians/s /// @@ -153,7 +177,8 @@ copy(std::string model, OutputIterator it, maxVel.push_back(2.356194490192); return boost::copy(maxVel, it); - } else if (boost::iequals(model, KUKA_LBR_IIWA_7_R800)) { + /// KUKA_LBR_IIWA_7_R800 to KUKA_LBR_IIWA_7_R820 + } else if (boost::iequals(model, "KUKA_LBR_IIWA_7_R800")) { /// @RK: updated the right joint velocity information based // on the 800 model from the KUKA manual @@ -203,7 +228,7 @@ class KukaUDP { }; enum ThreadingRunMode { run_manually = 0, run_automatically = 1 }; - + /// a fixed-size collection of heterogeneous values. typedef std::tuple Params; @@ -225,9 +250,9 @@ class KukaUDP { std::string remotehost(std::get(params)); std::string rp(std::get(params)); short remoteport = boost::lexical_cast(rp); - std::cout << "using: " - << " " << localhost << " " << localport << " " << remotehost - << " " << remoteport << "\n"; + // std::cout << "using: " + // << " " << localhost << " " << localport << " " << remotehost + // << " " << remoteport << "\n"; boost::asio::ip::udp::socket s( io_service_, diff --git a/include/grl/kuka/KukaDriver.hpp b/include/grl/kuka/KukaDriver.hpp index 06b209c..c31743c 100644 --- a/include/grl/kuka/KukaDriver.hpp +++ b/include/grl/kuka/KukaDriver.hpp @@ -29,8 +29,6 @@ namespace grl { namespace robot { namespace arm { - - /// /// /// @brief Kuka LBR iiwa Primary Multi Mode Driver, supports communication over FRI and JAVA interfaces @@ -71,7 +69,6 @@ namespace grl { namespace robot { namespace arm { std::string > Params; - static const Params defaultParams(){ return std::make_tuple( "Robotiiwa" , // RobotName, @@ -88,81 +85,72 @@ namespace grl { namespace robot { namespace arm { ); } - - KukaDriver(Params params = defaultParams()) - : params_(params), debug(false) - {} - - void construct(){ construct(params_);} - - bool destruct(){ return JAVAdriverP_->destruct(); } - - - /// @todo create a function that calls simGetObjectHandle and throws an exception when it fails - /// @warning getting the ik group is optional, so it does not throw an exception - void construct(Params params ) { - - params_ = params; - // keep driver threads from exiting immediately after creation, because they have work to do! - //device_driver_workP_.reset(new boost::asio::io_service::work(device_driver_io_service)); - - /// @todo figure out how to re-enable when .so isn't loaded - if( boost::iequals(std::get(params_),std::string("FRI")) - || boost::iequals(std::get(params_),std::string("FRI"))) - { - FRIdriverP_.reset( - new grl::robot::arm::KukaFRIdriver( - //device_driver_io_service, - std::make_tuple( - std::string(std::get (params)), - std::string(std::get (params)), - std::string(std::get (params)), - std::string(std::get (params)), - std::string(std::get (params)), - grl::robot::arm::KukaFRIClientDataDriver::run_automatically - ) - ) - - ); - FRIdriverP_->construct(); - } - - /// @todo implement reading configuration in both FRI and JAVA mode from JAVA interface - if( boost::iequals(std::get(params_),std::string("JAVA")) - || boost::iequals(std::get(params_),std::string("FRI"))) - { - try { - JAVAdriverP_ = boost::make_shared(params_); - JAVAdriverP_->construct(); - - // start up the driver thread - /// @todo perhaps allow user to control this? - //driver_threadP.reset(new std::thread([&]{ device_driver_io_service.run(); })); - } catch( boost::exception &e) { - e << errmsg_info("KukaDriver: Unable to connect to ZeroMQ Socket from " + - std::get (params_) + " to " + - std::get (params_)); - throw; + KukaDriver(Params params = defaultParams()) + : params_(params), debug(false) + {} + + void construct(){ construct(params_);} + + bool destruct(){ return JAVAdriverP_->destruct(); } + + + /// @todo create a function that calls simGetObjectHandle and throws an exception when it fails + /// @warning getting the ik group is optional, so it does not throw an exception + void construct(Params params ) { + params_ = params; + // keep driver threads from exiting immediately after creation, because they have work to do! + //device_driver_workP_.reset(new boost::asio::io_service::work(device_driver_io_service)); + + /// @todo figure out how to re-enable when .so isn't loaded + if( boost::iequals(std::get(params_),std::string("FRI")) + || boost::iequals(std::get(params_),std::string("FRI"))) + { + FRIdriverP_.reset( + new grl::robot::arm::KukaFRIdriver( + //device_driver_io_service, + std::make_tuple( + std::string(std::get (params)), + std::string(std::get (params)), + std::string(std::get (params)), + std::string(std::get (params)), + std::string(std::get (params)), + grl::robot::arm::KukaFRIClientDataDriver::run_automatically + ) + ) + ); + FRIdriverP_->construct(); + } + + /// @todo implement reading configuration in both FRI and JAVA mode from JAVA interface + if( boost::iequals(std::get(params_),std::string("JAVA")) + || boost::iequals(std::get(params_),std::string("FRI"))) + { + try { + JAVAdriverP_ = boost::make_shared(params_); + JAVAdriverP_->construct(); + + // start up the driver thread + /// @todo perhaps allow user to control this? + //driver_threadP.reset(new std::thread([&]{ device_driver_io_service.run(); })); + } catch( boost::exception &e) { + e << errmsg_info("KukaDriver: Unable to connect to ZeroMQ Socket from " + + std::get (params_) + " to " + + std::get (params_)); + throw; + } + } } + const Params & getParams(){ + return params_; } - } - - - - - - const Params & getParams(){ - return params_; - } - - ~KukaDriver(){ - device_driver_workP_.reset(); + ~KukaDriver(){ + device_driver_workP_.reset(); if(driver_threadP){ - device_driver_io_service.stop(); - driver_threadP->join(); + device_driver_io_service.stop(); + driver_threadP->join(); } } @@ -175,74 +163,109 @@ namespace grl { namespace robot { namespace arm { // @todo CHECK FOR REAL DATA BEFORE SENDING COMMANDS //if(!m_haveReceivedRealDataCount) return; bool haveNewData = false; - /// @todo make this handled by template driver implementations/extensions if(JAVAdriverP_.get() != nullptr) { - if (debug) { - std::cout << "commandedpos:" << armState_.commandedPosition << "\n"; - } + if (debug) { + std::cout << "commandedpos:" << armState_.commandedPosition << "\n"; + } - ///////////////////////////////////////// - // Do some configuration - if(boost::iequals(std::get(params_),std::string("FRI"))) - { - // configure to send commands over FRI interface - JAVAdriverP_->set(flatbuffer::KUKAiiwaInterface::FRI,command_tag()); - } + ///////////////////////////////////////// + // Do some configuration + if(boost::iequals(std::get(params_),std::string("FRI"))) + { + // configure to send commands over FRI interface + JAVAdriverP_->set(flatbuffer::KUKAiiwaInterface::FRI,command_tag()); + } - if(boost::iequals(std::get(params_),std::string("FRI"))) - { - // configure to send commands over FRI interface - JAVAdriverP_->set(flatbuffer::KUKAiiwaInterface::FRI,state_tag()); - } + if(boost::iequals(std::get(params_),std::string("FRI"))) + { + // configure to send commands over FRI interface + JAVAdriverP_->set(flatbuffer::KUKAiiwaInterface::FRI,state_tag()); + } - ///////////////////////////////////////// - // set new destination + ///////////////////////////////////////// + // set new destination - if( boost::iequals(std::get(params_),std::string("JAVA"))) - { - JAVAdriverP_->set(armState_.commandedPosition,revolute_joint_angle_open_chain_command_tag()); + if( boost::iequals(std::get(params_),std::string("JAVA"))) + { + JAVAdriverP_->set(armState_.commandedPosition,revolute_joint_angle_open_chain_command_tag()); - // configure to send commands over JAVA interface - JAVAdriverP_->set(flatbuffer::KUKAiiwaInterface::SmartServo,command_tag()); + // configure to send commands over JAVA interface + JAVAdriverP_->set(flatbuffer::KUKAiiwaInterface::SmartServo,command_tag()); - } + } - // sync JAVA driver with the robot, note client sends to server asynchronously! - haveNewData = JAVAdriverP_->run_one(); + // sync JAVA driver with the robot, note client sends to server asynchronously! + haveNewData = JAVAdriverP_->run_one(); - if( boost::iequals(std::get(params_),std::string("JAVA"))) - { - JAVAdriverP_->get(armState_); - JAVAdriverP_->set(flatbuffer::KUKAiiwaInterface::SmartServo,state_tag()); + if( boost::iequals(std::get(params_),std::string("JAVA"))) + { + JAVAdriverP_->get(armState_); + JAVAdriverP_->set(flatbuffer::KUKAiiwaInterface::SmartServo,state_tag()); - } + } } if(FRIdriverP_.get() != nullptr) { - if( boost::iequals(std::get(params_),std::string("FRI"))) - { - FRIdriverP_->set(armState_.commandedPosition,revolute_joint_angle_open_chain_command_tag()); - } - - haveNewData = FRIdriverP_->run_one(); - - if( boost::iequals(std::get(params_),std::string("FRI"))) - { - FRIdriverP_->get(armState_); - JAVAdriverP_->getWrench(armState_); - } + if( boost::iequals(std::get(params_),std::string("FRI"))) + { + FRIdriverP_->set(armState_.commandedPosition,revolute_joint_angle_open_chain_command_tag()); + } + + haveNewData = FRIdriverP_->run_one(); + + if( boost::iequals(std::get(params_),std::string("FRI"))) + { + FRIdriverP_->get(armState_); + //JAVAdriverP_->getWrench(armState_); + } } - return haveNewData; } + /// start recording the kuka state data in memory + /// return true on success, false on failure + bool start_recording(int single_buffer_limit_bytes) + { + if(FRIdriverP_.get() != nullptr) { + return FRIdriverP_->start_recording(single_buffer_limit_bytes); + } + return false; + } + /// stop recording the kuka state data in memory + /// return true on success, false on failure + bool stop_recording() + { + if(FRIdriverP_.get() != nullptr) { + return FRIdriverP_->stop_recording(); + } + return false; + } + bool save_recording(std::string filename = std::string()) { + if(FRIdriverP_.get() != nullptr) { + return FRIdriverP_->save_recording(filename); + } + return false; + } + void clear_recording() + { + if(FRIdriverP_.get() != nullptr) { + FRIdriverP_->clear_recording(); + } + } + bool is_recording() + { if(FRIdriverP_.get() != nullptr) { + // std::cout<< "In KukaDriver is_recording(): " << FRIdriverP_->is_recording() <is_recording(); + } + return false; + } /// set the mode of the arm. Examples: Teach or MoveArmJointServo /// @see grl::flatbuffer::ArmState in ArmControlState_generated.h void set(const flatbuffer::ArmState & armControlMode) @@ -263,49 +286,49 @@ namespace grl { namespace robot { namespace arm { } } - bool setPositionControlMode() - { - if(JAVAdriverP_) + bool setPositionControlMode() { - JAVAdriverP_->setPositionControlMode(); - return true; + if(JAVAdriverP_) + { + JAVAdriverP_->setPositionControlMode(); + return true; + } + else + return false; } - else - return false; - } - bool setJointImpedanceMode(std::vector joint_stiffnes, std::vectorjoint_damping) - { - if(JAVAdriverP_) + bool setJointImpedanceMode(std::vector joint_stiffnes, std::vectorjoint_damping) { - JAVAdriverP_->setJointImpedanceMode(joint_stiffnes, joint_damping); - return true; + if(JAVAdriverP_) + { + JAVAdriverP_->setJointImpedanceMode(joint_stiffnes, joint_damping); + return true; + } + else + return false; } - else - return false; - } - - // TODO: define custom flatbuffer for Cartesion Quantities, currently flatbuffer::CartesianImpedenceControlMode - bool setCartesianImpedanceMode( - grl::flatbuffer::EulerPose cart_stiffness, grl::flatbuffer::EulerPose cart_damping, - double nullspace_stiffness, double nullspace_damping, - grl::flatbuffer::EulerPose cart_max_path_deviation, - grl::flatbuffer::EulerPose cart_max_ctrl_vel, - grl::flatbuffer::EulerPose cart_max_ctrl_force, - bool max_control_force_stop) - { - if(JAVAdriverP_) + + // TODO: define custom flatbuffer for Cartesion Quantities, currently flatbuffer::CartesianImpedenceControlMode + bool setCartesianImpedanceMode( + grl::flatbuffer::EulerPose cart_stiffness, grl::flatbuffer::EulerPose cart_damping, + double nullspace_stiffness, double nullspace_damping, + grl::flatbuffer::EulerPose cart_max_path_deviation, + grl::flatbuffer::EulerPose cart_max_ctrl_vel, + grl::flatbuffer::EulerPose cart_max_ctrl_force, + bool max_control_force_stop) { - JAVAdriverP_->setCartesianImpedanceMode(cart_stiffness, cart_damping, - nullspace_stiffness, nullspace_damping, - cart_max_path_deviation, cart_max_ctrl_vel, cart_max_ctrl_force, - max_control_force_stop); - return true; + if(JAVAdriverP_) + { + JAVAdriverP_->setCartesianImpedanceMode(cart_stiffness, cart_damping, + nullspace_stiffness, nullspace_damping, + cart_max_path_deviation, cart_max_ctrl_vel, cart_max_ctrl_force, + max_control_force_stop); + return true; + } + else + return false; } - else - return false; - } /** * \brief Set the joint positions for the current interpolation step. @@ -321,8 +344,7 @@ namespace grl { namespace robot { namespace arm { armState_.clearCommands(); boost::copy(range, std::back_inserter(armState_.commandedPosition)); boost::copy(range, std::back_inserter(armState_.commandedPosition_goal)); - - std::cout << "set commandedpos:" << armState_.commandedPosition; + // std::cout << "set commandedpos:" << armState_.commandedPosition; } /** @@ -359,16 +381,16 @@ namespace grl { namespace robot { namespace arm { * */ void set(double duration_to_goal_command, time_duration_command_tag tag) { - boost::lock_guard lock(jt_mutex); - armState_.goal_position_command_time_duration = duration_to_goal_command; - if(FRIdriverP_) - { - FRIdriverP_->set(duration_to_goal_command,tag); - } - if(JAVAdriverP_) - { - JAVAdriverP_->set(duration_to_goal_command,tag); - } + boost::lock_guard lock(jt_mutex); + armState_.goal_position_command_time_duration = duration_to_goal_command; + if(FRIdriverP_) + { + FRIdriverP_->set(duration_to_goal_command,tag); + } + if(JAVAdriverP_) + { + JAVAdriverP_->set(duration_to_goal_command,tag); + } } @@ -380,9 +402,9 @@ namespace grl { namespace robot { namespace arm { * @see KukaFRIdriver::set(Range&& range, grl::revolute_joint_angle_open_chain_command_tag) * */ - KukaState::time_point_type get(time_point_tag) { + cartographer::common::Time get(time_point_tag) { boost::lock_guard lock(jt_mutex); - return armState_.timestamp; + return armState_.time_event_stamp.device_time; } /** @@ -447,13 +469,23 @@ namespace grl { namespace robot { namespace arm { } /// @todo(ahundt) replace with standard get() call with tag dispatch like above - template - void getWrench(OutputIterator output) - { - boost::unique_lock lock(jt_mutex); - boost::copy(armState_.wrenchJava,output); - - } + /// get 6 element wrench entries + /// [force_x, force_y, force_z, torque_x, torque_y, torque_z] + /// @param output a C++ OutputIterator which adds the values to your output object, + /// see http://en.cppreference.com/w/cpp/iterator/back_inserter + // template + // void getWrench(OutputIterator output) + // { + // boost::unique_lock lock(jt_mutex); + // JAVAdriverP_->getWrench(output); + // } + + template + void getWrench(Container output) + { + boost::unique_lock lock(jt_mutex); + JAVAdriverP_->getWrench(output); + } volatile std::size_t m_haveReceivedRealDataCount = 0; volatile std::size_t m_attemptedCommunicationCount = 0; diff --git a/include/grl/kuka/KukaFRIClientDataDriver.hpp b/include/grl/kuka/KukaFRIClientDataDriver.hpp new file mode 100644 index 0000000..739ab81 --- /dev/null +++ b/include/grl/kuka/KukaFRIClientDataDriver.hpp @@ -0,0 +1,861 @@ +/// KukaFRIDriver.hpp handles communication with the Kuka over FRI. +/// If you are new to this code base you are most likely looking for KukaDriver.hpp +#ifndef _KUKA_FRI_CLIENT_DATA_DRIVER +#define _KUKA_FRI_CLIENT_DATA_DRIVER + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +//#ifdef BOOST_NO_CXX11_ATOMIC_SMART_PTR +#include +//#endif +#include "grl/flatbuffer/flatbuffer.hpp" +// friClientData is found in the kuka connectivity FRI cpp zip file +#include "grl/kuka/Kuka.hpp" +#include "grl/kuka/KukaFRI.hpp" +#include "grl/exception.hpp" +#include "grl/vector_ostream.hpp" +#include "grl/kuka/KukaFRIalgorithm.hpp" +// used for time synchronization +#include "grl/TimeEvent.hpp" +#include "grl/time.hpp" + +// To call flatbuffer and toflatbuffer functions + +#include "grl/kuka/KukaToFlatbuffer.hpp" + +/// @todo TODO(ahundt) REMOVE SPDLOG FROM LOW LEVEL CODE +#include +#include + +struct KukaState; + +namespace grl { +namespace robot { +namespace arm { + +/// @brief internal function to decode KUKA FRI message buffer (using nanopb decoder) for the KUKA FRI +/// +/// @note encode needs to be updated for each additional supported command type +/// and when updating to newer FRI versions +void decode(KUKA::FRI::ClientData &friData, std::size_t msg_size) { + // The decoder was given a pointer to the monitoringMessage at initialization + // decode() is declared and definied in friMonitoringMessageDecoder.cpp (.h) + // All the stuffs in namespace KUKA::FRI are offered by KUKA side. + if (!friData.decoder.decode(friData.receiveBuffer, msg_size)) { + BOOST_THROW_EXCEPTION(std::runtime_error("Error decoding received FRI data, the message may be missing or corrupted. This error is most likely due to the application running on the KUKA robot's side of the connection going down or disabling FRI, so check the robot and the JAVA side of the system.")); + } + + // check message type + if (friData.expectedMonitorMsgID != friData.monitoringMsg.header.messageIdentifier) { + BOOST_THROW_EXCEPTION(std::invalid_argument( + std::string("KukaFRI.hpp: Problem reading buffer, id code: ") + + boost::lexical_cast( + static_cast(friData.monitoringMsg.header.messageIdentifier)) + + std::string(" does not match expected id code: ") + + boost::lexical_cast( + static_cast(friData.expectedMonitorMsgID)) + + std::string("\n"))); + return; + } + // ::SessionState + friData.lastState = static_cast(grl::robot::arm::get(friData.monitoringMsg, ::FRISessionState())); +} + +/// @brief Default LowLevelStepAlgorithmType +/// This algorithm is designed to be changed out +/// @todo Generalize this class using C++ techinques "tag dispatching" and "type +/// traits". See boost.geometry access and coorinate_type classes for examples. +/// Also perhaps make this the outer class which accepts drivers at the template param? +struct LinearInterpolation { + enum ParamIndex {JointAngleDest, TimeDurationToDestMS}; + + // A variable-size array container with fixed capacity. + // A static_vector is a sequence that supports random access to elements, constant time insertion and removal of elements at the end, and linear time insertion and removal of elements at the beginning or in the middle. + typedef std::tuple, std::size_t> Params; + // extremely conservative default timeframe to reach destination plus no goal position + static const Params defaultParams() { + boost::container::static_vector nopos; + return std::make_tuple(nopos,10000); + } + /// Default constructor + /// @todo verify this doesn't corrupt the state of the system + LinearInterpolation() { }; + // no action by default + template + void lowLevelTimestep(ArmDataType &, CommandModeType &) { + // need to tag dispatch here + BOOST_VERIFY(false); // not yet supported + } + + template + void lowLevelTimestep(ArmData &friData, revolute_joint_angle_open_chain_command_tag) { + + // no updates if no goal has been set + if(goal_position.size() == 0) return; + // switch (friData_->monitoringMsg.robotInfo.controlMode) { + // case ControlMode_POSITION_CONTROLMODE: + // case ControlMode_JOINT_IMPEDANCE_CONTROLMODE: + + KukaState::joint_state ipoJointPos; + KukaState::joint_state currentJointPos; + KukaState::joint_state currentMinusIPOJointPos; + KukaState::joint_state goalPlusIPOJointPos; + KukaState::joint_state diffToGoal; + KukaState::joint_state amountToMove; + KukaState::joint_state commandToSend; + KukaState::joint_state commandToSendPlusIPOJointPos; + + /// the current "holdposition" joint angles + /// @todo maybe this should be the revolute_joint_angle_interpolated_open_chain_state_tag()? @see + /// kukaFRIalgorithm.hpp + grl::robot::arm::copy(friData.monitoringMsg, + std::back_inserter(currentJointPos), + revolute_joint_angle_open_chain_state_tag()); + grl::robot::arm::copy(friData.monitoringMsg, + std::back_inserter(ipoJointPos), + revolute_joint_angle_interpolated_open_chain_state_tag()); + + boost::transform(currentJointPos, ipoJointPos, std::back_inserter(currentMinusIPOJointPos), std::minus()); + boost::transform(goal_position, ipoJointPos, std::back_inserter(goalPlusIPOJointPos), std::plus()); + + // only move if there is time left to reach the goal + if(goal_position_command_time_duration_remaining > 0) + { + /// single timestep in ms, the time duration between when udp packets are expected to be sent in milliseconds + /// uint32_t sendPeriod in ConnectionInfo; + int thisTimeStepMS(grl::robot::arm::get(friData.monitoringMsg, grl::time_step_tag())); + // std::cout << "Send Period: " << thisTimeStepMS << std::endl + // << "goal_position_command_time_duration_remaining: " <(thisTimeStepMS) / 1000); + + /// the fraction of the distance to the goal that should be traversed this tick + double fractionOfDistanceToTraverse = static_cast(thisTimeStepMS)/static_cast(goal_position_command_time_duration_remaining); + + // get the angular distance to the goal + // use current time and time to destination to interpolate (scale) goal + // joint position + boost::transform(goal_position, currentJointPos, std::back_inserter(diffToGoal), + [&](double commanded_angle, double current_angle) { + return (commanded_angle - current_angle) * fractionOfDistanceToTraverse; + }); + + + goal_position_command_time_duration_remaining -= thisTimeStepMS; + + /// @todo correctly pass velocity limits from outside, use "copy" fuction in + /// Kuka.hpp, correctly account for differing robot models. This *should* + /// be in KukaFRIdriver at the end of this file. + + // R820 velocity limits + // A1 - 85 °/s == 1.483529864195 rad/s + // A2 - 85 °/s == 1.483529864195 rad/s + // A3 - 100 °/s == 1.745329251994 rad/s + // A4 - 75 °/s == 1.308996938996 rad/s + // A5 - 130 °/s == 2.268928027593 rad/s + // A6 - 135 °/s == 2.356194490192 rad/s + // A1 - 135 °/s == 2.356194490192 rad/s + KukaState::joint_state velocity_limits; + velocity_limits.push_back(1.483529864195*thisTimeStepS); + velocity_limits.push_back(1.483529864195*thisTimeStepS); + velocity_limits.push_back(1.745329251994*thisTimeStepS); + velocity_limits.push_back(1.308996938996*thisTimeStepS); + velocity_limits.push_back(2.268928027593*thisTimeStepS); + velocity_limits.push_back(2.356194490192*thisTimeStepS); + velocity_limits.push_back(2.356194490192*thisTimeStepS); + + // clamp the commanded velocities to below the system limits + // use std::min to ensure commanded change in position remains under the + // maximum possible velocity for a single timestep + boost::transform(diffToGoal, velocity_limits, std::back_inserter(amountToMove), + [&](double diff, double maxvel) { + return boost::math::copysign(std::min(std::abs(diff), maxvel), diff); + }); + // add the current joint position to the amount to move to get the actual position command to send + boost::transform(currentJointPos, amountToMove, std::back_inserter(commandToSend), std::plus()); + boost::transform(commandToSend, ipoJointPos, std::back_inserter(commandToSendPlusIPOJointPos), std::plus()); + // send the command + grl::robot::arm::set(friData.commandMsg, commandToSend, grl::revolute_joint_angle_open_chain_command_tag()); + + // std::cout << "commandToSend: " << commandToSend << "\n" << + // "currentJointPos: " << currentJointPos << "\n" << + // "amountToMove: " << amountToMove << "\n" ; + + /// copy value for debugging, makes viewing in a debugger easier + double ripoJointPos[7]; + double rcurrentJointPos[7]; + double rcurrentMinusIPOJointPos[7]; + double rgoalPlusIPOJointPos[7]; + double rcommandedGoal[7]; + double rdiffToGoal[7]; + double ramountToMove[7]; + double rcommandToSend[7]; + double rvelocity_limits[7]; + double rcommandToSendPlusIPOJointPos[7]; + boost::copy(ipoJointPos, &ripoJointPos[0]); + boost::copy(currentJointPos, &rcurrentJointPos[0]); + boost::copy(goal_position, &rcommandedGoal[0]); + boost::copy(currentMinusIPOJointPos, &rcurrentMinusIPOJointPos[0]); + boost::copy(goal_position, &rcommandedGoal[0]); + boost::copy(goalPlusIPOJointPos, &rgoalPlusIPOJointPos[0]); + + boost::copy(goal_position, &rcommandedGoal[0]); + boost::copy(diffToGoal, &rdiffToGoal[0]); + boost::copy(velocity_limits, &rvelocity_limits[0]); + boost::copy(amountToMove, &ramountToMove[0]); + boost::copy(currentMinusIPOJointPos, &rcurrentMinusIPOJointPos[0]); + boost::copy(commandToSend, &rcommandToSend[0]); + boost::copy(commandToSendPlusIPOJointPos, &rcommandToSendPlusIPOJointPos[0]); + + } + } + + void setGoal(const Params& params ) { + /// @todo TODO(ahundt) support param tag structs for additional control modes + goal_position_command_time_duration_remaining = std::get(params); + goal_position = std::get(params); + } + + /// @todo look in FRI_Client_SDK_Cpp.zip to see if position must be set for + /// joint torques. Ref files: LBRTorqueSineOverlayClient.cpp, + /// LBRTorqueSineOverlayClient.h, friLBRCommand.cpp, friLBRCommand.h + template + void lowLevelTimestep(ArmData &friData, + revolute_joint_torque_open_chain_command_tag) { + //not yet supported + BOOST_VERIFY(false); + // case ControlMode_JOINT_IMPEDANCE_CONTROLMODE: + // grl::robot::arm::set(friData.commandMsg, armState.commandedTorque, + // grl::revolute_joint_torque_open_chain_command_tag()); + /// @note encode() needs to be updated for each additional supported command + /// type + // break; + } + + /// @todo look in FRI_Client_SDK_Cpp.zip to see if position must be set for + /// cartesian wrench. Ref files: LBRWrenchSineOverlayClient.cpp, + /// LBRWrenchSineOverlayClient.h, friLBRCommand.cpp, friLBRCommand.h + template + void lowLevelTimestep(ArmData &friData, cartesian_wrench_command_tag) { + //not yet supported + BOOST_VERIFY(false); + // case ControlMode_CARTESIAN_IMPEDANCE_CONTROLMODE: + // not yet supported + // grl::robot::arm::set(friData.commandMsg, + // armState.commandedCartesianWrenchFeedForward, + // grl::cartesian_wrench_command_tag()); + // break; + } + + /// @todo make this accessible via a nonmember function + bool hasCommandData() { + /// @todo check if duration remaining should be greater than zero or greater + /// than the last tick size + return goal_position_command_time_duration_remaining > 0; + } + private: + // the armstate at initialization of this object + KukaState::joint_state velocity_limits; + KukaState::joint_state goal_position; + double goal_position_command_time_duration_remaining; // milliseconds +}; // End of the structure + +/// @brief encode data in the class KUKA::FRI::ClientData into the send buffer for the KUKA FRI. +/// this preps the information for transport over the network +/// +/// @note encode needs to be updated for each additional supported command type +/// and when updating to newer FRI versions +template +std::size_t encode(LowLevelStepAlgorithmType &step_alg, + KUKA::FRI::ClientData &friData, + boost::system::error_code &ec) { + // reset send counter + friData.lastSendCounter = 0; + + // set sequence counters + friData.commandMsg.header.sequenceCounter = friData.sequenceCounter++; + friData.commandMsg.header.reflectedSequenceCounter = friData.monitoringMsg.header.sequenceCounter; + + ::FRISessionState sessionState = grl::robot::arm::get(friData.monitoringMsg, ::FRISessionState()); + + if ((step_alg.hasCommandData() && + (sessionState == ::FRISessionState::FRISessionState_COMMANDING_WAIT || sessionState == ::FRISessionState::FRISessionState_COMMANDING_ACTIVE))) + { + ::ClientCommandMode commandMode = grl::robot::arm::get(friData.monitoringMsg, ::ClientCommandMode()); + switch (commandMode) { + case ClientCommandMode_POSITION: + step_alg.lowLevelTimestep(friData, revolute_joint_angle_open_chain_command_tag()); + break; + case ClientCommandMode_WRENCH: + step_alg.lowLevelTimestep(friData, cartesian_wrench_command_tag()); + break; + case ClientCommandMode_TORQUE: + step_alg.lowLevelTimestep(friData, revolute_joint_torque_open_chain_command_tag()); + break; + default: + // this is unhandled at the moment... + BOOST_VERIFY(false); + // BOOST_THROW_EXCEPTION_CURRENT_FUNCTION; + // ClientCommandMode_NO_COMMAND_MODE, anything else that is added in the + // future and unimplemented + /// @todo do nothing if in an unsupported command mode? Or do the same as + /// the next else if step? + break; + } + + } else if (!(friData.commandMsg.has_commandData && step_alg.hasCommandData() && + (sessionState == ::FRISessionState::FRISessionState_COMMANDING_WAIT || sessionState == ::FRISessionState::FRISessionState_COMMANDING_ACTIVE))) + { + // copy current measured joint position to commanded position only if we + // *don't* have new command data + + /// @todo should this be different if it is in torque mode? + /// @todo allow copying of data directly between commandmsg and + /// monitoringMsg + std::vector msg; + copy(friData.monitoringMsg, std::back_inserter(msg), revolute_joint_angle_open_chain_command_tag()); + // copy the previously recorded command over + set(friData.commandMsg, msg, grl::revolute_joint_angle_open_chain_command_tag()); + } + + int buffersize = KUKA::FRI::FRI_COMMAND_MSG_MAX_SIZE; + if (!friData.encoder.encode(friData.sendBuffer, buffersize)) { + // @todo figure out PB_GET_ERROR, integrate with error_code type supported by boost + ec = boost::system::errc::make_error_code(boost::system::errc::bad_message); + return 0; + } + + return buffersize; +} + +/// @brief Actually talk over the network to receive an update and send out a new KUKA FRI command +/// +/// Receives an update, performs the necessary checks, then sends a message if appropriate. +/// +/// @pre socket must already have the endpoint resolved and "connected". While +/// udp is technically stateless the asio socket supports the connection api components for convenience. +template +void update_state(boost::asio::ip::udp::socket &socket, + LowLevelStepAlgorithmType &step_alg, + KUKA::FRI::ClientData &friData, + boost::system::error_code &receive_ec, + std::size_t &receive_bytes_transferred, + boost::system::error_code &send_ec, + std::size_t &send_bytes_transferred, + grl::TimeEvent& timeEvent, + boost::asio::ip::udp::endpoint sender_endpoint = boost::asio::ip::udp::endpoint()) { + + static const int message_flags = 0; + + // get a local clock timestamp, then the latest frame from the device, then another timestamp + timeEvent.local_request_time = cartographer::common::UniversalTimeScaleClock::now(); + receive_bytes_transferred = socket.receive_from( + boost::asio::buffer(friData.receiveBuffer, KUKA::FRI::FRI_MONITOR_MSG_MAX_SIZE), + sender_endpoint, message_flags, receive_ec); + timeEvent.local_receive_time = cartographer::common::UniversalTimeScaleClock::now(); + // int64_t request_time = cartographer::common::ToUniversal(timeEvent.local_request_time); + // int64_t receive_time = cartographer::common::ToUniversal(timeEvent.local_receive_time); + // assert(receive_time - request_time > 0); + decode(friData, receive_bytes_transferred); + + friData.lastSendCounter++; + // Check whether to send a response + if (friData.lastSendCounter >= friData.monitoringMsg.connectionInfo.receiveMultiplier) { + send_bytes_transferred = encode(step_alg, friData, send_ec); + if (send_ec) return; + socket.send(boost::asio::buffer(friData.sendBuffer, send_bytes_transferred), message_flags, send_ec); + } +} + + +/// @brief don't use this +/// @deprecated this is an old implemenation that will be removed in the future +void copy(const FRIMonitoringMessage &monitoringMsg, KukaState &state) { + state.clear(); + copy(monitoringMsg, std::back_inserter(state.position), + revolute_joint_angle_open_chain_state_tag()); + copy(monitoringMsg, std::back_inserter(state.torque), + revolute_joint_torque_open_chain_state_tag()); + copy(monitoringMsg, std::back_inserter(state.externalTorque), + grl::revolute_joint_torque_external_open_chain_state_tag()); + // only supported for kuka sunrise OS 1.9 + #ifdef KUKA_SUNRISE_1_9 + copy(friData_->monitoringMsg, std::back_inserter(state.externalForce), + grl::cartesian_external_force_tag()); + #endif // KUKA_SUNRISE_1_9 + copy(monitoringMsg, std::back_inserter(state.commandedPosition), + revolute_joint_angle_open_chain_command_tag()); + copy(monitoringMsg, std::back_inserter(state.commandedTorque), + revolute_joint_torque_open_chain_command_tag()); + copy(monitoringMsg, std::back_inserter(state.ipoJointPosition), + revolute_joint_angle_interpolated_open_chain_state_tag()); + state.sendPeriod = std::chrono::milliseconds( + get(monitoringMsg, grl::time_step_tag())); + state.sessionState = static_cast( + get(monitoringMsg, ::FRISessionState())); + state.connectionQuality = static_cast( + get(monitoringMsg, ::FRISessionState())); + state.safetyState = static_cast( + get(monitoringMsg, ::SafetyState())); + state.operationMode = static_cast( + get(monitoringMsg, ::OperationMode())); + state.driveState = static_cast( + get(monitoringMsg, ::DriveState())); + + /// @todo fill out missing state update steps +} + +/// @brief Simple low level driver to communicate over the Kuka iiwa FRI +/// interface using KUKA::FRI::ClientData status objects +/// +/// @note If you aren't sure, see KukaDriver in KukaDriver.hpp. +/// +/// @note If you want to change how the lowest level high rate updates are +/// performed see KukaFRIdriver +/// +/// One important aspect of this design is the is_running_automatically flag. If +/// you are unsure, +/// the suggested default is run_automatically (true/enabled). When it is +/// enabled, +/// the driver will create a thread internally and run the event loop +/// (io_service) itself. +/// If run manually, you are expected to call io_service.run() on the io_service +/// you provide, +/// or on the run() member function. When running manually you are also expected +/// to call +/// async_getLatestState(handler) frequently enought that the 5ms response +/// requirement of the KUKA +/// FRI interface is met. +template +class KukaFRIClientDataDriver + : public std::enable_shared_from_this< + KukaFRIClientDataDriver>, + public KukaUDP { + +public: + using KukaUDP::ParamIndex; + using KukaUDP::ThreadingRunMode; + using KukaUDP::Params; + using KukaUDP::defaultParams; + + KukaFRIClientDataDriver(boost::asio::io_service &ios, Params params = defaultParams()) + : params_(params), m_shouldStop(false), isConnectionEstablished_(false), + io_service_(ios) + //,socketP_(std::move(connect(params, io_service_,sender_endpoint_))) ///< + //@todo this breaks the assumption that the object can be constructed without + // hardware issues being a porblem + { + construct(params); + } + + KukaFRIClientDataDriver(Params params = defaultParams()) + : params_(params), m_shouldStop(false), isConnectionEstablished_(false), + optional_internal_io_service_P(new boost::asio::io_service), + io_service_(*optional_internal_io_service_P) + //,socketP_(std::move(connect(params, io_service_,sender_endpoint_))) ///< + //@todo this breaks the assumption that the object can be constructed without + // hardware issues being a porblem + { + construct(params); + } + + /// Call this to initialize the object after the constructor has been called + void construct(Params params = defaultParams()) { + try { + // initialize all of the states + latestStateForUser_ = make_valid_LatestState(); + spareStates_.emplace_back(std::move(make_valid_LatestState())); + spareStates_.emplace_back(std::move(make_valid_LatestState())); + + // start up the driver thread since the io_service_ is internal only + if (std::get(params)) { + driver_threadP_.reset(new std::thread([&] { update(); })); + } + + } catch (boost::exception &e) { + add_details_to_connection_error(e, params); + throw; + } + } + + /// @brief blocking call to communicate with the robot continuously + /// @pre construct() should be called before run() + void run() { update(); } + + /// @brief Updates the passed friData shared pointer to a pointer to newly + /// updated data, plus any errors that occurred. + /// + /// We recommend you supply a valid shared_ptr to friData, even if all command + /// elements are set to false. + /// The friData pointer you pass in can contain a command to send to the arm. + /// To update with new state and optional input state, you give up lifetime + /// control of the input, + /// and assume liftime control of the output. + /// + /// This function is designed for single threaded use to quickly receive and + /// send "non-blocking" updates to the robot. + /// It is not thread safe cannot be called simultaneously from multiple + /// threads. + /// + /// + /// @note { An error code is set if update_state is called with no new data + /// available. + /// In this special case, all error codes and bytes_transferred are 0, + /// because + /// there was no new data available for the user. + /// } + /// + /// @warning Do not pound this call continuously in a very tight loop, because + /// then the driver won't be able to acquire the lock and send updates to the + /// robot. + /// + /// @param[in,out] friData supply a new command, receive a new update of the + /// robot state. Pointer is null if no new data is available. + /// + /// @pre If friData!=nullptr it is assumed valid for use and this class will + /// take control of the object. + /// + /// @return isError = false if you have new data, true when there is either an + /// error or no new data + bool update_state(std::shared_ptr &step_alg_params, + std::shared_ptr &friData, + boost::system::error_code &receive_ec, + std::size_t &receive_bytes_transferred, + boost::system::error_code &send_ec, + std::size_t &send_bytes_transferred, + grl::TimeEvent& timeEvent) { + + if (exceptionPtr) { + /// @note this exception most likely came from the update() call running + /// the kuka driver + std::rethrow_exception(exceptionPtr); + } + + bool haveNewData = false; + + if (!isConnectionEstablished_ || !std::get(latestStateForUser_)) { + // no new data, so immediately return results accordingly + // Constructs a tuple object whose elements are references to the arguments in args std::tie (Types&... args) , in the same order. + // This allows a set of objects to act as a tuple, which is especially useful to unpack tuple objects. + std::tie(step_alg_params, friData, receive_ec, receive_bytes_transferred, send_ec, + send_bytes_transferred, timeEvent) = make_LatestState(step_alg_params,friData); + return !haveNewData; + } + + // ensure we have valid data for future updates + // need to copy this over because friData will be set as an output value later + // and allocate/initialize data if null + auto validFriDataLatestState = make_valid_LatestState(step_alg_params,friData); + + // get the latest state from the driver thread + { + boost::lock_guard lock(ptrMutex_); + + // get the update if one is available + // the user has provided new data to send to the device + if (std::get(validFriDataLatestState)->commandMsg.has_commandData) { + std::swap(validFriDataLatestState, newCommandForDriver_); + } + // newCommandForDriver_ is finalized + + if (spareStates_.size() < spareStates_.capacity() && + std::get(validFriDataLatestState)) { + spareStates_.emplace_back(std::move(validFriDataLatestState)); + } + + if (std::get(latestStateForUser_)) { + // return the latest state to the caller + std::tie(step_alg_params,friData, receive_ec, receive_bytes_transferred, send_ec, + send_bytes_transferred, timeEvent) = std::move(latestStateForUser_); + + haveNewData = true; + } else if (std::get( + validFriDataLatestState)) { + // all storage is full, return the spare data to the user + std::tie(step_alg_params, friData, receive_ec, receive_bytes_transferred, send_ec, + send_bytes_transferred, timeEvent) = validFriDataLatestState; + } + } + + // let the user know if we aren't in the best possible state + return !haveNewData || receive_bytes_transferred == 0 || receive_ec || send_ec; + } + + void destruct() { + m_shouldStop = true; + if (driver_threadP_) { + driver_threadP_->join(); + } + } + + ~KukaFRIClientDataDriver() { destruct(); } + + /// Is everything ok? + /// @return true if the kuka fri connection is actively running without any + /// issues + /// @todo consider expanding to support real error codes + bool is_active() { return !exceptionPtr && isConnectionEstablished_; } + +private: + /// Reads data off of the real kuka fri device in a separate thread + /// @todo consider switching to single producer single consumer queue to avoid + /// locking overhead, but keep latency in mind + /// https://github.com/facebook/folly/blob/master/folly/docs/ProducerConsumerQueue.md + void update() { + try { + + LowLevelStepAlgorithmType step_alg; + /// nextState is the object currently being loaded with data off the network + /// the driver thread should access this exclusively in update() + LatestState nextState = make_valid_LatestState(); + LatestState latestCommandBackup = make_valid_LatestState(); + /// Where is the sender_endpoint initialized? It should be bonded with the sender address and port. + boost::asio::ip::udp::endpoint sender_endpoint; + boost::asio::ip::udp::socket socket(connect(params_, io_service_, sender_endpoint)); + KukaState kukastate; ///< @todo TODO(ahundt) remove this line when new + /// api works completely since old one is deprecated + + ///////////// + // run the primary update loop in a separate thread + while (!m_shouldStop) { + /// @todo maybe there is a more convienient way to set this that is + /// easier for users? perhaps initializeClientDataForiiwa()? + + // nextState and latestCommandBackup should never be null + BOOST_VERIFY(std::get(nextState)); + BOOST_VERIFY(std::get(latestCommandBackup)); + + // set the flag that must always be there + std::get(nextState)->expectedMonitorMsgID = KUKA::LBRState::LBRMONITORMESSAGEID; + + auto lowLevelAlgorithmParamP = std::get(nextState); + + // if there is a valid low level algorithm param command set the new goal + if(lowLevelAlgorithmParamP) step_alg.setGoal(*lowLevelAlgorithmParamP); + + // actually talk over the network to receive an update and send out a new command + grl::robot::arm::update_state( + socket, step_alg, + *std::get(nextState), + std::get(nextState), + std::get(nextState), + std::get(nextState), + std::get(nextState), + std::get(nextState)); + + /// @todo use atomics to eliminate the global mutex lock for this object + // lock the mutex to communicate with the user thread + // if it cannot lock, simply send the previous message + // again + // bool try_lock(): Attempt to obtain ownership for the current thread without blocking. + if (ptrMutex_.try_lock()) { + ////////////////////////////////////////////// + // right now this is the state of everything: + ////////////////////////////////////////////// + // + // Always Valid: + // + // nextState: valid, contains the latest update + // latestCommandBackup: should always be valid (though hasCommand + // might be false) + // + // Either Valid or Null: + // latestStateForUser_ : null if the user took data out, valid otherwise + // newCommandForDriver_: null if there is no new command data from the user, vaild otherwise + + // 1) set the outgoing latest state for the user to pick up + // latestStateForUser_ is finalized + std::swap(latestStateForUser_, nextState); + + // 2) get a new incoming command if available and set incoming command + // variable to null + if (std::get(newCommandForDriver_)) { + // 3) back up the new incoming command + // latestCommandBackup is finalized, newCommandForDriver_ needs to be cleared out + std::swap(latestCommandBackup, newCommandForDriver_); + + // nextState may be null + if (!std::get(nextState)) { + nextState = std::move(newCommandForDriver_); + } else if (!(spareStates_.size() == spareStates_.capacity())) { + spareStates_.emplace_back(std::move(newCommandForDriver_)); + } else { + std::get(newCommandForDriver_).reset(); + } + } + + // finalized: latestStateForUser_, latestCommandBackup, + // newCommandForDriver_ is definitely null + // issues to be resolved: + // nextState: may be null right now, and it should be valid + // newCommandForDriver_: needs to be cleared with 100% certainty + BOOST_VERIFY(spareStates_.size() > 0); + + + if (!std::get(nextState) && + spareStates_.size()) { + // move the last element out and shorten the vector + nextState = std::move(*(spareStates_.end() - 1)); + spareStates_.pop_back(); + } + + BOOST_VERIFY(std::get(nextState)); + + KUKA::FRI::ClientData &nextClientData = + *std::get(nextState); + KUKA::FRI::ClientData &latestClientData = + *std::get(latestStateForUser_); + + // copy essential data from latestStateForUser_ to nextState + nextClientData.lastState = latestClientData.lastState; + nextClientData.sequenceCounter = latestClientData.sequenceCounter; + nextClientData.lastSendCounter = latestClientData.lastSendCounter; + nextClientData.expectedMonitorMsgID = latestClientData.expectedMonitorMsgID; + + // copy command from latestCommandBackup to nextState aka + // nextClientData + KUKA::FRI::ClientData &latestCommandBackupClientData = + *std::get(latestCommandBackup); + set(nextClientData.commandMsg, + latestCommandBackupClientData.commandMsg); + + // if there are no error codes and we have received data, + // then we can consider the connection established! + /// @todo perhaps data should always send too? + if (!std::get(nextState) && + !std::get(nextState) && + std::get(nextState)) { + isConnectionEstablished_ = true; + } + ptrMutex_.unlock(); + } + } + } catch (...) { + // transport the exception to the main thread in a safe manner + exceptionPtr = std::current_exception(); + m_shouldStop = true; + isConnectionEstablished_ = false; + } + + isConnectionEstablished_ = false; + } + + enum LatestStateIndex { + latest_low_level_algorithm_params, + latest_receive_monitor_state, + latest_receive_ec, + latest_receive_bytes_transferred, + latest_send_ec, + latest_send_bytes_transferred, + latest_time_event_data + }; + + /// this is the object that stores all data for the latest device state + /// including the KUKA defined ClientData object, and a grl defined TimeEvent + /// which stores the time data needed for synchronization. + typedef std::tuple, + std::shared_ptr, + boost::system::error_code, std::size_t, + boost::system::error_code, std::size_t, + grl::TimeEvent> LatestState; + + /// Creates a default LatestState Object + /// if there is no new data, then create an empty state. + static LatestState + make_LatestState(std::shared_ptr lowLevelAlgorithmParams, + std::shared_ptr &clientData) { + return std::make_tuple(lowLevelAlgorithmParams, + clientData, + boost::system::error_code(), std::size_t(), + boost::system::error_code(), std::size_t(), + grl::TimeEvent()); + } + + /// creates a shared_ptr to KUKA::FRI::ClientData with all command message + /// status explicitly set to false + /// @post std::shared_ptr will be non-null + static std::shared_ptr make_shared_valid_ClientData( + std::shared_ptr &friData) { + if (friData.get() == nullptr) { + friData = std::make_shared(KUKA::LBRState::NUM_DOF); + // there is no commandMessage data on a new object + friData->resetCommandMessage(); + } + + return friData; + } + + static std::shared_ptr make_shared_valid_ClientData() { + std::shared_ptr friData; + return make_shared_valid_ClientData(friData); + } + + /// Initialize valid shared ptr to LatestState object with a valid allocated + /// friData. Note that lowLevelAlgorithmParams will remain null! + static LatestState make_valid_LatestState( + std::shared_ptr lowLevelAlgorithmParams, + std::shared_ptr &friData) { + if (!friData) { + friData = make_shared_valid_ClientData(); + } + + return make_LatestState(lowLevelAlgorithmParams,friData); + } + + static LatestState make_valid_LatestState() { + std::shared_ptr emptyLowLevelAlgParams; + std::shared_ptr friData; + return make_valid_LatestState(emptyLowLevelAlgParams,friData); + } + + + Params params_; + + /// @todo replace with unique_ptr + /// the latest state we have available to give to the user + LatestState latestStateForUser_; + LatestState newCommandForDriver_; + + /// should always be valid, never null + boost::container::static_vector spareStates_; + + std::atomic m_shouldStop; + std::exception_ptr exceptionPtr; + std::atomic isConnectionEstablished_; + + /// may be null, allows the user to choose if they want to provide an + /// io_service + std::unique_ptr optional_internal_io_service_P; + + // other things to do somewhere: + // - get list of control points + // - get the control point in the arm base coordinate system + // - load up a configuration file with ip address to send to, etc. + boost::asio::io_service &io_service_; + std::unique_ptr driver_threadP_; + boost::mutex ptrMutex_; + + typename LowLevelStepAlgorithmType::Params step_alg_params_; +}; /// End of KukaFRIClientDataDriver + +} +} +} // namespace grl::robot::arm + +#endif diff --git a/include/grl/kuka/KukaFRIalgorithm.hpp b/include/grl/kuka/KukaFRIalgorithm.hpp index 5b135e3..6d18935 100644 --- a/include/grl/kuka/KukaFRIalgorithm.hpp +++ b/include/grl/kuka/KukaFRIalgorithm.hpp @@ -28,14 +28,13 @@ namespace grl { namespace robot { - namespace arm { namespace kuka { - // Following from Kuka example program - const int default_port_id = 30200; - struct send_period{}; - struct receive_multiplier{}; + // Following from Kuka example program + const int default_port_id = 30200; + struct send_period{}; + struct receive_multiplier{}; namespace detail { @@ -78,13 +77,13 @@ namespace grl { namespace robot { /// copy measured joint angle to output iterator template void copy(const FRIMonitoringMessage& monitoringMsg, OutputIterator it, revolute_joint_angle_open_chain_state_tag){ - kuka::detail::copyJointState(monitoringMsg.monitorData.measuredJointPosition.value.arg,it,monitoringMsg.monitorData.has_measuredJointPosition); + kuka::detail::copyJointState(monitoringMsg.monitorData.measuredJointPosition.value.arg,it,monitoringMsg.monitorData.has_measuredJointPosition); } /// copy interpolated commanded joint angles template void copy(const FRIMonitoringMessage& monitoringMsg, OutputIterator it,revolute_joint_angle_interpolated_open_chain_state_tag){ - kuka::detail::copyJointState(monitoringMsg.ipoData.jointPosition.value.arg,it,monitoringMsg.ipoData.has_jointPosition); + kuka::detail::copyJointState(monitoringMsg.ipoData.jointPosition.value.arg,it,monitoringMsg.ipoData.has_jointPosition); } /// copy commanded joint angle to output iterator @@ -97,13 +96,13 @@ namespace grl { namespace robot { /// copy measured joint torque to output iterator template void copy(const FRIMonitoringMessage& monitoringMsg, OutputIterator it, revolute_joint_torque_open_chain_state_tag){ - kuka::detail::copyJointState(monitoringMsg.monitorData.measuredTorque.value.arg,it, monitoringMsg.monitorData.has_measuredTorque); + kuka::detail::copyJointState(monitoringMsg.monitorData.measuredTorque.value.arg,it, monitoringMsg.monitorData.has_measuredTorque); } /// copy measured external joint torque to output iterator template void copy(const FRIMonitoringMessage& monitoringMsg, OutputIterator it, revolute_joint_torque_external_open_chain_state_tag){ - kuka::detail::copyJointState(monitoringMsg.monitorData.externalTorque.value.arg,it, monitoringMsg.monitorData.has_externalTorque); + kuka::detail::copyJointState(monitoringMsg.monitorData.externalTorque.value.arg,it, monitoringMsg.monitorData.has_externalTorque); } // only supported for kuka sunrise OS 1.9 @@ -120,75 +119,70 @@ namespace grl { namespace robot { /// (Euler angles A, B, C) of the currently used motion center. template void copy(const FRIMonitoringMessage& monitoringMsg, OutputIterator it, cartesian_external_force_tag){ - kuka::detail::copyCartesianState(monitoringMsg.monitorData.externalForce,it, monitoringMsg.monitorData.has_externalForce); - } + kuka::detail::copyCartesianState(monitoringMsg.monitorData.externalForce,it, monitoringMsg.monitorData.has_externalForce); + } #endif // KUKA_SUNRISE_1_9 /// copy commanded joint torque to output iterator template void copy(const FRIMonitoringMessage& monitoringMsg, OutputIterator it, revolute_joint_torque_open_chain_command_tag){ - kuka::detail::copyJointState(monitoringMsg.monitorData.commandedTorque.value.arg,it, monitoringMsg.monitorData.has_commandedTorque); + kuka::detail::copyJointState(monitoringMsg.monitorData.commandedTorque.value.arg,it, monitoringMsg.monitorData.has_commandedTorque); } - - + /// copy commanded joint angle to output iterator template void copy(const FRICommandMessage& commandMsg, OutputIterator it, revolute_joint_angle_open_chain_command_tag){ if (commandMsg.has_commandData) kuka::detail::copyJointState(commandMsg.commandData.jointPosition.value.arg,it,commandMsg.commandData.has_jointPosition); } + /// ::SafetyState is defined in #include "friClientIf.h" + /// @todo consider using another default value, or perhaps boost::optional<::SafetyState>? + ::SafetyState get(const FRIMonitoringMessage& monitoringMsg, const ::SafetyState) { + ::SafetyState state = ::SafetyState::SafetyState_NORMAL_OPERATION; - /// @todo consider using another default value, or perhaps boost::optional? - KUKA::FRI::ESafetyState get(const FRIMonitoringMessage& monitoringMsg, const KUKA::FRI::ESafetyState) { - KUKA::FRI::ESafetyState state = KUKA::FRI::ESafetyState::NORMAL_OPERATION; if (monitoringMsg.has_robotInfo) { if (monitoringMsg.robotInfo.has_safetyState) - return static_cast(monitoringMsg.robotInfo.safetyState); + return monitoringMsg.robotInfo.safetyState; } return state; } - - KUKA::FRI::EOperationMode get(const FRIMonitoringMessage& monitoringMsg, const KUKA::FRI::EOperationMode) { - KUKA::FRI::EOperationMode state = KUKA::FRI::EOperationMode::TEST_MODE_1; + ::OperationMode get(const FRIMonitoringMessage& monitoringMsg, const ::OperationMode) { + ::OperationMode state = ::OperationMode::OperationMode_TEST_MODE_1; if (monitoringMsg.has_robotInfo) { if (monitoringMsg.robotInfo.has_operationMode) - state = static_cast(monitoringMsg.robotInfo.operationMode); + state = monitoringMsg.robotInfo.operationMode; } return state; } - - KUKA::FRI::EControlMode get(const FRIMonitoringMessage& monitoringMsg, const KUKA::FRI::EControlMode) { - KUKA::FRI::EControlMode state = KUKA::FRI::EControlMode::NO_CONTROL; + ::ControlMode get(const FRIMonitoringMessage& monitoringMsg, const ::ControlMode) { + ::ControlMode state = ::ControlMode::ControlMode_NO_CONTROLMODE; if (monitoringMsg.has_robotInfo) { if (monitoringMsg.robotInfo.has_controlMode) - state = static_cast(monitoringMsg.robotInfo.controlMode); + state = monitoringMsg.robotInfo.controlMode; } return state; } - - KUKA::FRI::EClientCommandMode get(const FRIMonitoringMessage& monitoringMsg, const KUKA::FRI::EClientCommandMode) { - KUKA::FRI::EClientCommandMode state = KUKA::FRI::EClientCommandMode::NO_COMMAND_MODE; + ::ClientCommandMode get(const FRIMonitoringMessage& monitoringMsg, const ::ClientCommandMode) { + ::ClientCommandMode state = ::ClientCommandMode::ClientCommandMode_NO_COMMAND_MODE; if (monitoringMsg.has_ipoData) { if (monitoringMsg.ipoData.has_clientCommandMode) - state = static_cast(monitoringMsg.ipoData.clientCommandMode); + state = monitoringMsg.ipoData.clientCommandMode; } return state; } - - KUKA::FRI::EOverlayType get(const FRIMonitoringMessage& monitoringMsg, const KUKA::FRI::EOverlayType) { - KUKA::FRI::EOverlayType state = KUKA::FRI::EOverlayType::NO_OVERLAY; + ::OverlayType get(const FRIMonitoringMessage& monitoringMsg, const ::OverlayType) { + ::OverlayType state = ::OverlayType::OverlayType_NO_OVERLAY; if (monitoringMsg.has_ipoData) { if (monitoringMsg.ipoData.has_overlayType) - state = static_cast(monitoringMsg.ipoData.overlayType); + state = monitoringMsg.ipoData.overlayType; } return state; } - - KUKA::FRI::EDriveState get(const FRIMonitoringMessage& _message, const KUKA::FRI::EDriveState) + ::DriveState get(const FRIMonitoringMessage& _message, const ::DriveState) { tRepeatedIntArguments *values = (tRepeatedIntArguments *)_message.robotInfo.driveState.arg; @@ -198,24 +192,24 @@ namespace grl { namespace robot { int state = (int)values->value[i]; if (state != firstState) { - return KUKA::FRI::EDriveState::TRANSITIONING; + return ::DriveState::DriveState_TRANSITIONING; } } - return (KUKA::FRI::EDriveState)firstState; + return (::DriveState)firstState; } - KUKA::FRI::ESessionState get(const FRIMonitoringMessage& monitoringMsg, const KUKA::FRI::ESessionState){ - KUKA::FRI::ESessionState KukaSessionState = KUKA::FRI::ESessionState::IDLE; + ::FRISessionState get(const FRIMonitoringMessage& monitoringMsg, const ::FRISessionState){ + ::FRISessionState KukaSessionState = ::FRISessionState::FRISessionState_IDLE; if (monitoringMsg.has_connectionInfo) { - KukaSessionState = static_cast(monitoringMsg.connectionInfo.sessionState); + KukaSessionState = monitoringMsg.connectionInfo.sessionState; } return KukaSessionState; } - KUKA::FRI::EConnectionQuality get(const FRIMonitoringMessage& monitoringMsg, const KUKA::FRI::EConnectionQuality){ - KUKA::FRI::EConnectionQuality KukaQuality = KUKA::FRI::EConnectionQuality::POOR; + ::FRIConnectionQuality get(const FRIMonitoringMessage& monitoringMsg, const ::FRIConnectionQuality){ + ::FRIConnectionQuality KukaQuality = ::FRIConnectionQuality::FRIConnectionQuality_POOR; if (monitoringMsg.has_connectionInfo) { - KukaQuality = static_cast(monitoringMsg.connectionInfo.quality); + KukaQuality = monitoringMsg.connectionInfo.quality; } return KukaQuality; } @@ -233,7 +227,16 @@ namespace grl { namespace robot { return KukaSendPeriod; } - std::size_t get(const FRIMonitoringMessage& monitoringMsg,const kuka::receive_multiplier){ + std::size_t get(const FRIMonitoringMessage& monitoringMsg,const std::size_t NUM_DOF){ + std::size_t numberofJoints = 7; + if (monitoringMsg.has_robotInfo) { + if (monitoringMsg.robotInfo.has_numberOfJoints) + return monitoringMsg.robotInfo.numberOfJoints; + } + return numberofJoints; + } + + std::size_t get(const FRIMonitoringMessage& monitoringMsg,const kuka::receive_multiplier){ std::size_t KukaReceiveMultiplier = 0; if (monitoringMsg.has_connectionInfo) { if (monitoringMsg.connectionInfo.has_receiveMultiplier) @@ -251,16 +254,14 @@ namespace grl { namespace robot { } return timestamp; } - - - /** - * \brief Set the joint positions for the current interpolation step. - * - * This method is only effective when the client is in a commanding state. - * @param state Object which stores the current state of the robot, including the command to send next - * @param range Array with the new joint positions (in radians) - * @param tag identifier object indicating that revolute joint angle commands should be modified - */ + /** + * \brief Set the joint positions for the current interpolation step. + * + * This method is only effective when the client is in a commanding state. + * @param state Object which stores the current state of the robot, including the command to send next + * @param range Array with the new joint positions (in radians) + * @param tag identifier object indicating that revolute joint angle commands should be modified + */ template static inline void set(FRICommandMessage & state, Range&& range, grl::revolute_joint_angle_open_chain_command_tag) { if(boost::size(range)) @@ -272,17 +273,17 @@ namespace grl { namespace robot { } } - /** - * \brief Set the applied joint torques for the current interpolation step. - * - * This method is only effective when the client is in a commanding state. - * The ControlMode of the robot has to be joint impedance control mode. The - * Client Command Mode has to be torque. - * - * @param state Object which stores the current state of the robot, including the command to send next - * @param torques Array with the applied torque values (in Nm) - * @param tag identifier object indicating that the torqe value command should be modified - */ + /** + * \brief Set the applied joint torques for the current interpolation step. + * + * This method is only effective when the client is in a commanding state. + * The ControlMode of the robot has to be joint impedance control mode. The + * Client Command Mode has to be torque. + * + * @param state Object which stores the current state of the robot, including the command to send next + * @param torques Array with the applied torque values (in Nm) + * @param tag identifier object indicating that the torqe value command should be modified + */ template static inline void set(FRICommandMessage & state, Range&& range, grl::revolute_joint_torque_open_chain_command_tag) { if(boost::size(range)) @@ -295,74 +296,70 @@ namespace grl { namespace robot { } - /** - * \brief Set the applied wrench vector of the current interpolation step. - * - * The wrench vector consists of: - * [F_x, F_y, F_z, tau_A, tau_B, tau_C] - * - * F ... forces (in N) applied along the Cartesian axes of the - * currently used motion center. - * tau ... torques (in Nm) applied along the orientation angles - * (Euler angles A, B, C) of the currently used motion center. - * - * This method is only effective when the client is in a commanding state. - * The ControlMode of the robot has to be Cartesian impedance control mode. The - * Client Command Mode has to be wrench. - * - * @param FRICommandMessage object storing the command data that will be sent to the physical device - * @param range wrench Applied Cartesian wrench vector, in x, y, z, roll, pitch, yaw force measurments. - * @param tag identifier object indicating that the wrench value command should be modified - * - * @todo perhaps support some specific more useful data layouts - * @note copies only the elements that will fit - */ + /** + * \brief Set the applied wrench vector of the current interpolation step. + * + * The wrench vector consists of: + * [F_x, F_y, F_z, tau_A, tau_B, tau_C] + * + * F ... forces (in N) applied along the Cartesian axes of the + * currently used motion center. + * tau ... torques (in Nm) applied along the orientation angles + * (Euler angles A, B, C) of the currently used motion center. + * + * This method is only effective when the client is in a commanding state. + * The ControlMode of the robot has to be Cartesian impedance control mode. The + * Client Command Mode has to be wrench. + * + * @param FRICommandMessage object storing the command data that will be sent to the physical device + * @param range wrench Applied Cartesian wrench vector, in x, y, z, roll, pitch, yaw force measurments. + * @param tag identifier object indicating that the wrench value command should be modified + * + * @todo perhaps support some specific more useful data layouts + * @note copies only the elements that will fit + */ template static inline void set(FRICommandMessage & state, Range&& range, grl::cartesian_wrench_command_tag) { - if(boost::size(range)) - { - state.has_commandData = true; - state.commandData.has_cartesianWrenchFeedForward = true; - std::copy_n(std::begin(range),std::min(boost::size(range),state.commandData.cartesianWrenchFeedForward.element_count), &state.commandData.cartesianWrenchFeedForward.element[0]); - } + if(boost::size(range)) + { + state.has_commandData = true; + state.commandData.has_cartesianWrenchFeedForward = true; + std::copy_n(std::begin(range),std::min(boost::size(range),state.commandData.cartesianWrenchFeedForward.element_count), &state.commandData.cartesianWrenchFeedForward.element[0]); + } } /// set the left destination FRICommandMessage state to be equal to the right source FRICommandMessage state static inline void set(FRICommandMessage & state, const FRICommandMessage& sourceState, grl::command_tag) { - state.has_commandData = sourceState.has_commandData; - - // cartesianWrench - state.commandData.has_cartesianWrenchFeedForward = state.commandData.has_cartesianWrenchFeedForward; - std::copy_n(&state.commandData.cartesianWrenchFeedForward.element[0],std::min(state.commandData.cartesianWrenchFeedForward.element_count,sourceState.commandData.cartesianWrenchFeedForward.element_count), &state.commandData.cartesianWrenchFeedForward.element[0]); - - // for joint copying - tRepeatedDoubleArguments *dest; - tRepeatedDoubleArguments *source; - + state.has_commandData = sourceState.has_commandData; + // cartesianWrench + state.commandData.has_cartesianWrenchFeedForward = state.commandData.has_cartesianWrenchFeedForward; + std::copy_n(&state.commandData.cartesianWrenchFeedForward.element[0],std::min(state.commandData.cartesianWrenchFeedForward.element_count,sourceState.commandData.cartesianWrenchFeedForward.element_count), &state.commandData.cartesianWrenchFeedForward.element[0]); - // jointTorque - state.commandData.has_jointTorque = state.commandData.has_jointTorque; - dest = (tRepeatedDoubleArguments*)state.commandData.jointTorque.value.arg; - source = (tRepeatedDoubleArguments*)sourceState.commandData.jointTorque.value.arg; - std::copy_n(source->value,std::min(source->size,dest->size), dest->value); + // for joint copying + tRepeatedDoubleArguments *dest; + tRepeatedDoubleArguments *source; + // jointTorque + state.commandData.has_jointTorque = state.commandData.has_jointTorque; + dest = (tRepeatedDoubleArguments*)state.commandData.jointTorque.value.arg; + source = (tRepeatedDoubleArguments*)sourceState.commandData.jointTorque.value.arg; + std::copy_n(source->value,std::min(source->size,dest->size), dest->value); - // jointPosition - state.commandData.has_jointPosition = state.commandData.has_jointPosition; - dest = (tRepeatedDoubleArguments*)state.commandData.jointPosition.value.arg; - source = (tRepeatedDoubleArguments*)sourceState.commandData.jointPosition.value.arg; - std::copy_n(source->value,std::min(source->size,dest->size), dest->value); - } + // jointPosition + state.commandData.has_jointPosition = state.commandData.has_jointPosition; + dest = (tRepeatedDoubleArguments*)state.commandData.jointPosition.value.arg; + source = (tRepeatedDoubleArguments*)sourceState.commandData.jointPosition.value.arg; + std::copy_n(source->value,std::min(source->size,dest->size), dest->value); + } static inline void set(FRICommandMessage & state, const FRICommandMessage& sourceState) { - set(state,sourceState, grl::command_tag()); + set(state,sourceState, grl::command_tag()); } - }}} // namespace grl::robot::arm @@ -406,8 +403,6 @@ namespace traits { template struct dimension : boost::mpl::int_ {}; template struct coordinate_type { typedef boost::iterator_range type; }; - - // template struct coordinate_type; // template struct coordinate_type { typedef boost::iterator_range type; }; // template struct coordinate_type { typedef boost::iterator_range type; }; @@ -457,12 +452,6 @@ namespace traits { //static inline void set(nrec::spatial::Coordinate & p, typename nrec::spatial::Coordinate::value_type const& value) { p.operator[](Index) = value; } - - - - - - // // angle // const typename coordinate_type::type // get(FRIMonitoringMessage const& monitoringMsg,revolute_joint_angle_open_chain_state_tag) { @@ -502,24 +491,10 @@ namespace traits { }; - - - - - - - - - - - - template struct tag { typedef grl::device_command_tag type; }; template struct dimension : boost::mpl::int_<1> {}; // each joint is 1 dimensional template struct coordinate_type { typedef boost::iterator_range type; }; - - // template struct coordinate_type; // template struct coordinate_type { typedef boost::iterator_range type; }; // template struct coordinate_type { typedef boost::iterator_range type; }; @@ -536,23 +511,17 @@ namespace traits { // return grl::robot::arm::kuka::detail::get(*static_cast(monitoringMsg.monitorData.commandedJointPosition.value.arg),monitoringMsg.monitorData.has_measuredJointPosition); // } - template - static inline void set(FRICommandMessage & state, Range&& range, grl::revolute_joint_angle_open_chain_command_tag) { - state.has_commandData = true; - state.commandData.has_jointPosition = true; - tRepeatedDoubleArguments *dest = (tRepeatedDoubleArguments*)state.commandData.jointPosition.value.arg; - boost::copy(range, dest->value); - } - - + template + static inline void set(FRICommandMessage & state, Range&& range, grl::revolute_joint_angle_open_chain_command_tag) { + state.has_commandData = true; + state.commandData.has_jointPosition = true; + tRepeatedDoubleArguments *dest = (tRepeatedDoubleArguments*)state.commandData.jointPosition.value.arg; + boost::copy(range, dest->value); + } }; - }}} // boost::geometry::traits - - - #endif diff --git a/include/grl/kuka/KukaFRIdriver.hpp b/include/grl/kuka/KukaFRIdriver.hpp index 7708d9c..b785746 100644 --- a/include/grl/kuka/KukaFRIdriver.hpp +++ b/include/grl/kuka/KukaFRIdriver.hpp @@ -2,35 +2,12 @@ /// If you are new to this code base you are most likely looking for KukaDriver.hpp #ifndef _KUKA_FRI_DRIVER #define _KUKA_FRI_DRIVER +#define FLATBUFFERS_DEBUG_VERIFICATION_FAILURE -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -//#ifdef BOOST_NO_CXX11_ATOMIC_SMART_PTR -#include -//#endif - -// friClientData is found in the kuka connectivity FRI cpp zip file -#include "grl/kuka/Kuka.hpp" -#include "grl/kuka/KukaFRI.hpp" -#include "grl/exception.hpp" -#include "grl/vector_ostream.hpp" -#include "grl/kuka/KukaFRIalgorithm.hpp" - - -/// @todo TODO(ahundt) REMOVE SPDLOG FROM LOW LEVEL CODE -#include -#include +#include "grl/kuka/KukaFRIClientDataDriver.hpp" +#include +#include +//#include "cartographer/common/time.h" struct KukaState; @@ -38,853 +15,6 @@ namespace grl { namespace robot { namespace arm { -/// @brief internal function to decode KUKA FRI message buffer (using nanopb -/// decoder) for the KUKA FRI -/// -/// @note encode needs to be updated for each additional supported command type -/// and when updating to newer FRI versions -void decode(KUKA::FRI::ClientData &friData, std::size_t msg_size) { - // The decoder was given a pointer to the monitoringMessage at initialization - if (!friData.decoder.decode(friData.receiveBuffer, msg_size)) { - BOOST_THROW_EXCEPTION(std::runtime_error("Error decoding received FRI data, the message may be missing or corrupted. This error is most likely due to the application running on the KUKA robot's side of the connection going down or disabling FRI, so check the robot and the JAVA side of the system.")); - } - - // check message type - if (friData.expectedMonitorMsgID != - friData.monitoringMsg.header.messageIdentifier) { - BOOST_THROW_EXCEPTION(std::invalid_argument( - std::string("KukaFRI.hpp: Problem reading buffer, id code: ") + - boost::lexical_cast( - static_cast(friData.monitoringMsg.header.messageIdentifier)) + - std::string(" does not match expected id code: ") + - boost::lexical_cast( - static_cast(friData.expectedMonitorMsgID)) + - std::string("\n"))); - return; - } - - friData.lastState = - grl::robot::arm::get(friData.monitoringMsg, KUKA::FRI::ESessionState()); -} - -/// @brief Default LowLevelStepAlgorithmType -/// This algorithm is designed to be changed out -/// @todo Generalize this class using C++ techinques "tag dispatching" and "type -/// traits". See boost.geometry access and coorinate_type classes for examples. -/// Also perhaps make this the outer class which accepts drivers at the template param? -struct LinearInterpolation { - - enum ParamIndex { - JointAngleDest, - TimeDurationToDestMS - }; - - typedef std::tuple,std::size_t> Params; - - // extremely conservative default timeframe to reach destination plus no goal position - static const Params defaultParams() { - boost::container::static_vector nopos; - return std::make_tuple(nopos,10000); - } - /// Default constructor - /// @todo verify this doesn't corrupt the state of the system - LinearInterpolation() { - }; - - // no action by default - template - void lowLevelTimestep(ArmDataType &, CommandModeType &) { - // need to tag dispatch here - BOOST_VERIFY(false); // not yet supported - } - - template - void lowLevelTimestep(ArmData &friData, - revolute_joint_angle_open_chain_command_tag) { - - // no updates if no goal has been set - if(goal_position.size() == 0) return; - // switch (friData_->monitoringMsg.robotInfo.controlMode) { - // case ControlMode_POSITION_CONTROLMODE: - // case ControlMode_JOINT_IMPEDANCE_CONTROLMODE: - - KukaState::joint_state ipoJointPos; - KukaState::joint_state currentJointPos; - KukaState::joint_state currentMinusIPOJointPos; - KukaState::joint_state goalPlusIPOJointPos; - KukaState::joint_state diffToGoal; - KukaState::joint_state amountToMove; - KukaState::joint_state commandToSend; - KukaState::joint_state commandToSendPlusIPOJointPos; - - double ripoJointPos[7]; - double rcurrentJointPos[7]; - double rcurrentMinusIPOJointPos[7]; - double rgoalPlusIPOJointPos[7]; - double rcommandedGoal[7]; - double rdiffToGoal[7]; - double ramountToMove[7]; - double rcommandToSend[7]; - double rvelocity_limits[7]; - double rcommandToSendPlusIPOJointPos[7]; - - // the current "holdposition" joint angles - /// @todo maybe this should be the - /// revolute_joint_angle_interpolated_open_chain_state_tag()? @see - /// kukaFRIalgorithm.hpp - grl::robot::arm::copy(friData.monitoringMsg, - std::back_inserter(currentJointPos), - revolute_joint_angle_open_chain_state_tag()); - grl::robot::arm::copy(friData.monitoringMsg, - std::back_inserter(ipoJointPos), - revolute_joint_angle_interpolated_open_chain_state_tag()); - - // copy value for debugging - boost::copy(ipoJointPos, &ripoJointPos[0]); - boost::copy(currentJointPos, &rcurrentJointPos[0]); - boost::copy(goal_position, &rcommandedGoal[0]); - - boost::transform(currentJointPos, ipoJointPos, - std::back_inserter(currentMinusIPOJointPos), std::minus()); - boost::transform(goal_position, ipoJointPos, - std::back_inserter(goalPlusIPOJointPos), std::plus()); - - - boost::copy(currentMinusIPOJointPos, &rcurrentMinusIPOJointPos[0]); - boost::copy(goal_position, &rcommandedGoal[0]); - boost::copy(goalPlusIPOJointPos, &rgoalPlusIPOJointPos[0]); - - // only move if there is time left to reach the goal - if(goal_position_command_time_duration_remaining > 0) - { - // single timestep in ms - int thisTimeStepMS(grl::robot::arm::get(friData.monitoringMsg, grl::time_step_tag())); - double thisTimeStepS = (static_cast(thisTimeStepMS) / 1000); - //double secondsPerTick = std::chrono::duration_cast(thisTimeStep).count(); - - // the fraction of the distance to the goal that should be traversed this - // tick - double fractionOfDistanceToTraverse = - static_cast(thisTimeStepMS) / - static_cast(goal_position_command_time_duration_remaining); - - // makes viewing in a debugger easier - boost::copy(goal_position, &rcommandedGoal[0]); - // get the angular distance to the goal - // use current time and time to destination to interpolate (scale) goal - // joint position - boost::transform(goal_position, currentJointPos, - std::back_inserter(diffToGoal), - [&](double commanded_angle, double current_angle) { - return (commanded_angle - current_angle) * - fractionOfDistanceToTraverse; - }); - boost::copy(diffToGoal, &rdiffToGoal[0]); - - goal_position_command_time_duration_remaining -= thisTimeStepMS; - - /// @todo correctly pass velocity limits from outside, use "copy" fuction in - /// Kuka.hpp, correctly account for differing robot models. This *should* - /// be in KukaFRIdriver at the end of this file. - - // R820 velocity limits - // A1 - 85 °/s == 1.483529864195 rad/s - // A2 - 85 °/s == 1.483529864195 rad/s - // A3 - 100 °/s == 1.745329251994 rad/s - // A4 - 75 °/s == 1.308996938996 rad/s - // A5 - 130 °/s == 2.268928027593 rad/s - // A6 - 135 °/s == 2.356194490192 rad/s - // A1 - 135 °/s == 2.356194490192 rad/s - KukaState::joint_state velocity_limits; - velocity_limits.push_back(1.483529864195*thisTimeStepS); - velocity_limits.push_back(1.483529864195*thisTimeStepS); - velocity_limits.push_back(1.745329251994*thisTimeStepS); - velocity_limits.push_back(1.308996938996*thisTimeStepS); - velocity_limits.push_back(2.268928027593*thisTimeStepS); - velocity_limits.push_back(2.356194490192*thisTimeStepS); - velocity_limits.push_back(2.356194490192*thisTimeStepS); - - boost::copy(velocity_limits, &rvelocity_limits[0]); - // clamp the commanded velocities to below the system limits - // use std::min to ensure commanded change in position remains under the - // maximum possible velocity for a single timestep - boost::transform( - diffToGoal, velocity_limits, std::back_inserter(amountToMove), - [&](double diff, double maxvel) { - return boost::math::copysign(std::min(std::abs(diff), maxvel), diff); - }); - - boost::copy(amountToMove, &ramountToMove[0]); - - - // add the current joint position to the amount to move to get the actual - // position command to send - boost::transform(currentJointPos, amountToMove, - std::back_inserter(commandToSend), std::plus()); - - boost::copy(currentMinusIPOJointPos, &rcurrentMinusIPOJointPos[0]); - boost::copy(commandToSend, &rcommandToSend[0]); - - - boost::transform(commandToSend, ipoJointPos, - std::back_inserter(commandToSendPlusIPOJointPos), std::plus()); - - - boost::copy(commandToSendPlusIPOJointPos, &rcommandToSendPlusIPOJointPos[0]); - // send the command -// grl::robot::arm::set(friData.commandMsg, commandToSend, -// grl::revolute_joint_angle_open_chain_command_tag()); - // send the command - grl::robot::arm::set(friData.commandMsg, commandToSend, - grl::revolute_joint_angle_open_chain_command_tag()); - } - // break; - } - - void setGoal(const Params& params ) { - /// @todo TODO(ahundt) support param tag structs for additional control modes - goal_position_command_time_duration_remaining = std::get(params); - goal_position = std::get(params); - - } - - /// @todo look in FRI_Client_SDK_Cpp.zip to see if position must be set for - /// joint torques. Ref files: LBRTorqueSineOverlayClient.cpp, - /// LBRTorqueSineOverlayClient.h, friLBRCommand.cpp, friLBRCommand.h - template - void lowLevelTimestep(ArmData &friData, - revolute_joint_torque_open_chain_command_tag) { - - //not yet supported - BOOST_VERIFY(false); - // case ControlMode_JOINT_IMPEDANCE_CONTROLMODE: -// grl::robot::arm::set(friData.commandMsg, armState.commandedTorque, -// grl::revolute_joint_torque_open_chain_command_tag()); - - /// @note encode() needs to be updated for each additional supported command - /// type - // break; - } - - /// @todo look in FRI_Client_SDK_Cpp.zip to see if position must be set for - /// cartesian wrench. Ref files: LBRWrenchSineOverlayClient.cpp, - /// LBRWrenchSineOverlayClient.h, friLBRCommand.cpp, friLBRCommand.h - template - void lowLevelTimestep(ArmData &friData, cartesian_wrench_command_tag) { - - //not yet supported - BOOST_VERIFY(false); - // case ControlMode_CARTESIAN_IMPEDANCE_CONTROLMODE: - // not yet supported -// grl::robot::arm::set(friData.commandMsg, -// armState.commandedCartesianWrenchFeedForward, -// grl::cartesian_wrench_command_tag()); - // break; - } - - /// @todo make this accessible via a nonmember function - bool hasCommandData() { - /// @todo check if duration remaining should be greater than zero or greater - /// than the last tick size - return goal_position_command_time_duration_remaining > 0; - } - // template - // void operator()(ArmData& clientData, - // revolute_joint_angle_open_chain_command_tag){ - // default: - // break; - // } -private: - // the armstate at initialization of this object - KukaState::joint_state velocity_limits; - KukaState::joint_state goal_position; - double goal_position_command_time_duration_remaining; // milliseconds -}; - -/// @brief encode data in the class KUKA::FRI::ClientData into the send buffer -/// for the KUKA FRI. -/// this preps the information for transport over the network -/// -/// @note encode needs to be updated for each additional supported command type -/// and when updating to newer FRI versions -template -std::size_t encode(LowLevelStepAlgorithmType &step_alg, - KUKA::FRI::ClientData &friData, - boost::system::error_code &ec) { - // reset send counter - friData.lastSendCounter = 0; - - // set sequence counters - friData.commandMsg.header.sequenceCounter = friData.sequenceCounter++; - friData.commandMsg.header.reflectedSequenceCounter = - friData.monitoringMsg.header.sequenceCounter; - - KUKA::FRI::ESessionState sessionState = - grl::robot::arm::get(friData.monitoringMsg, KUKA::FRI::ESessionState()); - - if ((step_alg.hasCommandData() && - (sessionState == KUKA::FRI::COMMANDING_WAIT || - sessionState == KUKA::FRI::COMMANDING_ACTIVE))) { - KUKA::FRI::EClientCommandMode commandMode = grl::robot::arm::get( - friData.monitoringMsg, KUKA::FRI::EClientCommandMode()); - switch (commandMode) { - case ClientCommandMode_POSITION: - step_alg.lowLevelTimestep(friData, revolute_joint_angle_open_chain_command_tag()); - break; - case ClientCommandMode_WRENCH: - step_alg.lowLevelTimestep(friData, cartesian_wrench_command_tag()); - break; - case ClientCommandMode_TORQUE: - step_alg.lowLevelTimestep(friData, revolute_joint_torque_open_chain_command_tag()); - break; - default: - // this is unhandled at the moment... - BOOST_VERIFY(false); - // BOOST_THROW_EXCEPTION_CURRENT_FUNCTION; - // ClientCommandMode_NO_COMMAND_MODE, anything else that is added in the - // future and unimplemented - /// @todo do nothing if in an unsupported command mode? Or do the same as - /// the next else if step? - break; - } - - } else if (!(friData.commandMsg.has_commandData && - step_alg.hasCommandData() && - (sessionState == KUKA::FRI::COMMANDING_WAIT || - sessionState == KUKA::FRI::COMMANDING_ACTIVE))) { - // copy current measured joint position to commanded position only if we - // *don't* have new command data - - /// @todo should this be different if it is in torque mode? - /// @todo allow copying of data directly between commandmsg and - /// monitoringMsg - std::vector msg; - copy(friData.monitoringMsg, std::back_inserter(msg), - revolute_joint_angle_open_chain_command_tag()); - // copy the previously recorded command over - set(friData.commandMsg, msg, - grl::revolute_joint_angle_open_chain_command_tag()); - } - - int buffersize = KUKA::FRI::FRI_COMMAND_MSG_MAX_SIZE; - if (!friData.encoder.encode(friData.sendBuffer, buffersize)) { - // @todo figure out PB_GET_ERROR, integrate with error_code type supported - // by boost - ec = boost::system::errc::make_error_code(boost::system::errc::bad_message); - return 0; - } - - return buffersize; -} - -/// @brief Actually talk over the network to receive an update and send out a -/// new KUKA FRI command -/// -/// Receives an update, performs the necessary checks, then sends a message if -/// appropriate. -/// -/// @pre socket must already have the endpoint resolved and "connected". While -/// udp is technically stateless the asio socket supports the connection api -/// components for convenience. -template -void update_state(boost::asio::ip::udp::socket &socket, - LowLevelStepAlgorithmType &step_alg, - KUKA::FRI::ClientData &friData, - boost::system::error_code &receive_ec, - std::size_t &receive_bytes_transferred, - boost::system::error_code &send_ec, - std::size_t &send_bytes_transferred, - boost::asio::ip::udp::endpoint sender_endpoint = - boost::asio::ip::udp::endpoint()) { - - static const int message_flags = 0; - receive_bytes_transferred = socket.receive_from( - boost::asio::buffer(friData.receiveBuffer, - KUKA::FRI::FRI_MONITOR_MSG_MAX_SIZE), - sender_endpoint, message_flags, receive_ec); - decode(friData, receive_bytes_transferred); - - friData.lastSendCounter++; - // Check whether to send a response - if (friData.lastSendCounter >= - friData.monitoringMsg.connectionInfo.receiveMultiplier) { - send_bytes_transferred = encode(step_alg, friData, send_ec); - if (send_ec) - return; - socket.send(boost::asio::buffer(friData.sendBuffer, send_bytes_transferred), - message_flags, send_ec); - } -} - - -/// @brief don't use this -/// @deprecated this is an old implemenation that will be removed in the future -void copy(const FRIMonitoringMessage &monitoringMsg, KukaState &state) { - state.clear(); - copy(monitoringMsg, std::back_inserter(state.position), - revolute_joint_angle_open_chain_state_tag()); - copy(monitoringMsg, std::back_inserter(state.torque), - revolute_joint_torque_open_chain_state_tag()); - copy(monitoringMsg, std::back_inserter(state.commandedPosition), - revolute_joint_angle_open_chain_command_tag()); - copy(monitoringMsg, std::back_inserter(state.commandedTorque), - revolute_joint_torque_open_chain_command_tag()); - copy(monitoringMsg, std::back_inserter(state.ipoJointPosition), - revolute_joint_angle_interpolated_open_chain_state_tag()); - state.sessionState = static_cast( - get(monitoringMsg, KUKA::FRI::ESessionState())); - state.connectionQuality = static_cast( - get(monitoringMsg, KUKA::FRI::EConnectionQuality())); - state.safetyState = static_cast( - get(monitoringMsg, KUKA::FRI::ESafetyState())); - state.operationMode = static_cast( - get(monitoringMsg, KUKA::FRI::EOperationMode())); - state.driveState = static_cast( - get(monitoringMsg, KUKA::FRI::EDriveState())); - - /// @todo fill out missing state update steps -} - -/// @brief Simple low level driver to communicate over the Kuka iiwa FRI -/// interface using KUKA::FRI::ClientData status objects -/// -/// @note If you aren't sure, see KukaDriver in KukaDriver.hpp. -/// -/// @note If you want to change how the lowest level high rate updates are -/// performed see KukaFRIdriver -/// -/// One important aspect of this design is the is_running_automatically flag. If -/// you are unsure, -/// the suggested default is run_automatically (true/enabled). When it is -/// enabled, -/// the driver will create a thread internally and run the event loop -/// (io_service) itself. -/// If run manually, you are expected to call io_service.run() on the io_service -/// you provide, -/// or on the run() member function. When running manually you are also expected -/// to call -/// async_getLatestState(handler) frequently enought that the 5ms response -/// requirement of the KUKA -/// FRI interface is met. -template -class KukaFRIClientDataDriver - : public std::enable_shared_from_this< - KukaFRIClientDataDriver>, - public KukaUDP { - -public: - using KukaUDP::ParamIndex; - using KukaUDP::ThreadingRunMode; - using KukaUDP::Params; - using KukaUDP::defaultParams; - - KukaFRIClientDataDriver(boost::asio::io_service &ios, - Params params = defaultParams()) - : params_(params), m_shouldStop(false), isConnectionEstablished_(false), - io_service_(ios) - //,socketP_(std::move(connect(params, io_service_,sender_endpoint_))) ///< - //@todo this breaks the assumption that the object can be constructed without - // hardware issues being a porblem - { - construct(params); - } - - KukaFRIClientDataDriver(Params params = defaultParams()) - : params_(params), m_shouldStop(false), isConnectionEstablished_(false), - optional_internal_io_service_P(new boost::asio::io_service), - io_service_(*optional_internal_io_service_P) - //,socketP_(std::move(connect(params, io_service_,sender_endpoint_))) ///< - //@todo this breaks the assumption that the object can be constructed without - // hardware issues being a porblem - { - construct(params); - } - - /// Call this to initialize the object after the constructor has been called - void construct(Params params = defaultParams()) { - try { - - /////////// - // initialize all of the states - latestStateForUser_ = make_valid_LatestState(); - spareStates_.emplace_back(std::move(make_valid_LatestState())); - spareStates_.emplace_back(std::move(make_valid_LatestState())); - - // start up the driver thread since the io_service_ is internal only - if (std::get(params)) { - driver_threadP_.reset(new std::thread([&] { update(); })); - } - - } catch (boost::exception &e) { - add_details_to_connection_error(e, params); - throw; - } - } - - /// @brief blocking call to communicate with the robot continuously - /// @pre construct() should be called before run() - void run() { update(); } - - /// @brief Updates the passed friData shared pointer to a pointer to newly - /// updated data, plus any errors that occurred. - /// - /// We recommend you supply a valid shared_ptr to friData, even if all command - /// elements are set to false. - /// The friData pointer you pass in can contain a command to send to the arm. - /// To update with new state and optional input state, you give up lifetime - /// control of the input, - /// and assume liftime control of the output. - /// - /// This function is designed for single threaded use to quickly receive and - /// send "non-blocking" updates to the robot. - /// It is not thread safe cannot be called simultaneously from multiple - /// threads. - /// - /// - /// @note { An error code is set if update_state is called with no new data - /// available. - /// In this special case, all error codes and bytes_transferred are 0, - /// because - /// there was no new data available for the user. - /// } - /// - /// @warning Do not pound this call continuously in a very tight loop, because - /// then the driver won't be able to acquire the lock and send updates to the - /// robot. - /// - /// @param[in,out] friData supply a new command, receive a new update of the - /// robot state. Pointer is null if no new data is available. - /// - /// @pre If friData!=nullptr it is assumed valid for use and this class will - /// take control of the object. - /// - /// @return isError = false if you have new data, true when there is either an - /// error or no new data - bool update_state(std::shared_ptr &step_alg_params, - std::shared_ptr &friData, - boost::system::error_code &receive_ec, - std::size_t &receive_bytes_transferred, - boost::system::error_code &send_ec, - std::size_t &send_bytes_transferred) { - - if (exceptionPtr) { - /// @note this exception most likely came from the update() call running - /// the kuka driver - std::rethrow_exception(exceptionPtr); - } - - bool haveNewData = false; - - if (!isConnectionEstablished_ || - !std::get(latestStateForUser_)) { - // no new data, so immediately return results accordingly - std::tie(step_alg_params, friData, receive_ec, receive_bytes_transferred, send_ec, - send_bytes_transferred) = make_LatestState(step_alg_params,friData); - return !haveNewData; - } - - // ensure we have valid data for future updates - // need to copy this over because friData will be set as an output value - // later - // and allocate/initialize data if null - auto validFriDataLatestState = make_valid_LatestState(step_alg_params,friData); - - // get the latest state from the driver thread - { - boost::lock_guard lock(ptrMutex_); - - // get the update if one is available - // the user has provided new data to send to the device - if (std::get(validFriDataLatestState) - ->commandMsg.has_commandData) { - std::swap(validFriDataLatestState, newCommandForDriver_); - } - // newCommandForDriver_ is finalized - - if (spareStates_.size() < spareStates_.capacity() && - std::get(validFriDataLatestState)) { - spareStates_.emplace_back(std::move(validFriDataLatestState)); - } - - if (std::get(latestStateForUser_)) { - // return the latest state to the caller - std::tie(step_alg_params,friData, receive_ec, receive_bytes_transferred, send_ec, - send_bytes_transferred) = std::move(latestStateForUser_); - haveNewData = true; - } else if (std::get( - validFriDataLatestState)) { - // all storage is full, return the spare data to the user - std::tie(step_alg_params, friData, receive_ec, receive_bytes_transferred, send_ec, - send_bytes_transferred) = validFriDataLatestState; - } - } - - // let the user know if we aren't in the best possible state - return !haveNewData || receive_bytes_transferred == 0 || receive_ec || - send_ec; - } - - void destruct() { - m_shouldStop = true; - if (driver_threadP_) { - driver_threadP_->join(); - } - } - - ~KukaFRIClientDataDriver() { destruct(); } - - /// Is everything ok? - /// @return true if the kuka fri connection is actively running without any - /// issues - /// @todo consider expanding to support real error codes - bool is_active() { return !exceptionPtr && isConnectionEstablished_; } - -private: - /// Reads data off of the real kuka fri device in a separate thread - /// @todo consider switching to single producer single consumer queue to avoid - /// locking overhead, but keep latency in mind - /// https://github.com/facebook/folly/blob/master/folly/docs/ProducerConsumerQueue.md - void update() { - try { - - LowLevelStepAlgorithmType step_alg; - /// nextState is the object currently being loaded with data off the - /// network - /// the driver thread should access this exclusively in update() - LatestState nextState = make_valid_LatestState(); - LatestState latestCommandBackup = make_valid_LatestState(); - - boost::asio::ip::udp::endpoint sender_endpoint; - boost::asio::ip::udp::socket socket( - connect(params_, io_service_, sender_endpoint)); - KukaState kukastate; ///< @todo TODO(ahundt) remove this line when new - /// api works completely since old one is deprecated - - ///////////// - // run the primary update loop in a separate thread - while (!m_shouldStop) { - /// @todo maybe there is a more convienient way to set this that is - /// easier for users? perhaps initializeClientDataForiiwa()? - - // nextState and latestCommandBackup should never be null - BOOST_VERIFY(std::get(nextState)); - BOOST_VERIFY( - std::get(latestCommandBackup)); - - // set the flag that must always be there - std::get(nextState) - ->expectedMonitorMsgID = KUKA::LBRState::LBRMONITORMESSAGEID; - - auto lowLevelAlgorithmParamP = std::get(nextState); - - // if there is a valid low level algorithm param command set the new goal - if(lowLevelAlgorithmParamP) step_alg.setGoal(*lowLevelAlgorithmParamP); - - // actually talk over the network to receive an update and send out a - // new command - grl::robot::arm::update_state( - socket, step_alg, - *std::get(nextState), - std::get(nextState), - std::get(nextState), - std::get(nextState), - std::get(nextState)); - - /// @todo use atomics to eliminate the global mutex lock for this object - // lock the mutex to communicate with the user thread - // if it cannot lock, simply send the previous message - // again - if (ptrMutex_.try_lock()) { - - ////////////////////////////////////////////// - // right now this is the state of everything: - ////////////////////////////////////////////// - // - // Always Valid: - // - // nextState: valid, contains the latest update - // latestCommandBackup: should always be valid (though hasCommand - // might be false) - // - // Either Valid or Null: - // latestStateForUser_ : null if the user took data out, valid - // otherwise - // newCommandForDriver_: null if there is no new command data from - // the user, vaild otherwise - - // 1) set the outgoing latest state for the user to pick up - // latestStateForUser_ is finalized - std::swap(latestStateForUser_, nextState); - - // 2) get a new incoming command if available and set incoming command - // variable to null - if (std::get(newCommandForDriver_)) { - // 3) back up the new incoming command - // latestCommandBackup is finalized, newCommandForDriver_ needs to - // be cleared out - std::swap(latestCommandBackup, newCommandForDriver_); - - // nextState may be null - if (!std::get(nextState)) { - nextState = std::move(newCommandForDriver_); - } else if (!(spareStates_.size() == spareStates_.capacity())) { - spareStates_.emplace_back(std::move(newCommandForDriver_)); - } else { - std::get(newCommandForDriver_) - .reset(); - } - } - - // finalized: latestStateForUser_, latestCommandBackup, - // newCommandForDriver_ is definitely null - // issues to be resolved: - // nextState: may be null right now, and it should be valid - // newCommandForDriver_: needs to be cleared with 100% certainty - BOOST_VERIFY(spareStates_.size() > 0); - - if (!std::get(nextState) && - spareStates_.size()) { - // move the last element out and shorten the vector - nextState = std::move(*(spareStates_.end() - 1)); - spareStates_.pop_back(); - } - - BOOST_VERIFY(std::get(nextState)); - - KUKA::FRI::ClientData &nextClientData = - *std::get(nextState); - KUKA::FRI::ClientData &latestClientData = - *std::get(latestStateForUser_); - - // copy essential data from latestStateForUser_ to nextState - nextClientData.lastState = latestClientData.lastState; - nextClientData.sequenceCounter = latestClientData.sequenceCounter; - nextClientData.lastSendCounter = latestClientData.lastSendCounter; - nextClientData.expectedMonitorMsgID = - latestClientData.expectedMonitorMsgID; - - // copy command from latestCommandBackup to nextState aka - // nextClientData - KUKA::FRI::ClientData &latestCommandBackupClientData = - *std::get(latestCommandBackup); - set(nextClientData.commandMsg, - latestCommandBackupClientData.commandMsg); - - // if there are no error codes and we have received data, - // then we can consider the connection established! - /// @todo perhaps data should always send too? - if (!std::get(nextState) && - !std::get(nextState) && - std::get(nextState)) { - isConnectionEstablished_ = true; - } - - ptrMutex_.unlock(); - } - } - - } catch (...) { - // transport the exception to the main thread in a safe manner - exceptionPtr = std::current_exception(); - m_shouldStop = true; - isConnectionEstablished_ = false; - } - - isConnectionEstablished_ = false; - } - - enum LatestStateIndex { - latest_low_level_algorithm_params, - latest_receive_monitor_state, - latest_receive_ec, - latest_receive_bytes_transferred, - latest_send_ec, - latest_send_bytes_transferred - }; - - typedef std::tuple, - std::shared_ptr, - boost::system::error_code, std::size_t, - boost::system::error_code, std::size_t> - LatestState; - - /// Creates a default LatestState Object - static LatestState - make_LatestState(std::shared_ptr lowLevelAlgorithmParams, - std::shared_ptr &clientData) { - return std::make_tuple(lowLevelAlgorithmParams, clientData, boost::system::error_code(), - std::size_t(), boost::system::error_code(), - std::size_t()); - } - - /// creates a shared_ptr to KUKA::FRI::ClientData with all command message - /// status explicitly set to false - /// @post std::shared_ptr will be non-null - static std::shared_ptr make_shared_valid_ClientData( - std::shared_ptr &friData) { - if (friData.get() == nullptr) { - friData = - std::make_shared(KUKA::LBRState::NUM_DOF); - // there is no commandMessage data on a new object - friData->resetCommandMessage(); - } - - return friData; - } - - static std::shared_ptr make_shared_valid_ClientData() { - std::shared_ptr friData; - return make_shared_valid_ClientData(friData); - } - - /// Initialize valid shared ptr to LatestState object with a valid allocated - /// friData. Note that lowLevelAlgorithmParams will remain null! - static LatestState - make_valid_LatestState( - std::shared_ptr lowLevelAlgorithmParams, - std::shared_ptr &friData - ) { - if (!friData) - friData = make_shared_valid_ClientData(); - - return make_LatestState(lowLevelAlgorithmParams,friData); - } - - static LatestState make_valid_LatestState() { - std::shared_ptr emptyLowLevelAlgParams; - std::shared_ptr friData; - return make_valid_LatestState(emptyLowLevelAlgParams,friData); - } - - Params params_; - - /// @todo replace with unique_ptr - /// the latest state we have available to give to the user - LatestState latestStateForUser_; - LatestState newCommandForDriver_; - - /// should always be valid, never null - boost::container::static_vector spareStates_; - - std::atomic m_shouldStop; - std::exception_ptr exceptionPtr; - std::atomic isConnectionEstablished_; - - /// may be null, allows the user to choose if they want to provide an - /// io_service - std::unique_ptr optional_internal_io_service_P; - - // other things to do somewhere: - // - get list of control points - // - get the control point in the arm base coordinate system - // - load up a configuration file with ip address to send to, etc. - boost::asio::io_service &io_service_; - std::unique_ptr driver_threadP_; - boost::mutex ptrMutex_; - - typename LowLevelStepAlgorithmType::Params step_alg_params_; -}; - /// @brief Primary Kuka FRI driver, only talks over realtime network FRI KONI /// ethernet port /// @@ -926,409 +56,777 @@ class KukaFRIClientDataDriver /// so that the low level update algorithm can change. /// @todo add getter for number of milliseconds between fri updates (1-5) aka /// sync_period aka send_period aka ms per tick -template -class KukaFRIdriver : public std::enable_shared_from_this< - KukaFRIdriver>, - public KukaUDP { +/// std::enable_shared_from_this allows an object t that is currently managed by +/// a std::shared_ptr named pt to safely generate additional std::shared_ptr instances pt1, pt2, ... +/// that all share ownership of t with pt. +template +class KukaFRIdriver : public std::enable_shared_from_this>, public KukaUDP +{ public: - using KukaUDP::ParamIndex; - using KukaUDP::ThreadingRunMode; - using KukaUDP::Params; - using KukaUDP::defaultParams; - - KukaFRIdriver(Params params = defaultParams()) : params_(params) {} - - // KukaFRIdriver(boost::asio::io_service& - // device_driver_io_service__,Params params = defaultParams()) - // : - // device_driver_io_service(device_driver_io_service__), - // params_(params) - // {} - - void construct() { construct(params_); } - - /// @todo create a function that calls simGetObjectHandle and throws an - /// exception when it fails - /// @warning getting the ik group is optional, so it does not throw an - /// exception - void construct(Params params) { - - params_ = params; - // keep driver threads from exiting immediately after creation, because they - // have work to do! - device_driver_workP_.reset( - new boost::asio::io_service::work(device_driver_io_service)); - - kukaFRIClientDataDriverP_.reset( - new grl::robot::arm::KukaFRIClientDataDriver( - device_driver_io_service, - std::make_tuple(std::string(std::get(params)), - std::string(std::get(params)), - std::string(std::get(params)), - std::string(std::get(params)), - std::string(std::get(params)), - grl::robot::arm::KukaFRIClientDataDriver< - LowLevelStepAlgorithmType>::run_automatically)) - - ); - } + using KukaUDP::ParamIndex; // enum, define some connection parameters, such as ip, port... + using KukaUDP::ThreadingRunMode; + using KukaUDP::Params; // std::tuple, contains the information needed to connect to the robot. + using KukaUDP::defaultParams; // A method to assign Params with defaut values. + + KukaFRIdriver(Params params = defaultParams()) : params_(params), m_shouldStop(false) {} + + void construct() { construct(params_); } + + /// @todo create a function that calls simGetObjectHandle and throws an + /// exception when it fails + /// @warning getting the ik group is optional, so it does not throw an + /// exception + void construct(Params params) { + params_ = params; + // keep driver threads from exiting immediately after creation, because they have work to do! + // boost::asio::io_service::work: Constructor notifies the io_service that work is starting. + device_driver_workP_.reset(new boost::asio::io_service::work(device_driver_io_service)); + + kukaFRIClientDataDriverP_.reset( + new grl::robot::arm::KukaFRIClientDataDriver( + device_driver_io_service, + std::make_tuple(std::string(std::get(params)), + std::string(std::get(params)), + std::string(std::get(params)), + std::string(std::get(params)), + std::string(std::get(params)), + grl::robot::arm::KukaFRIClientDataDriver< + LowLevelStepAlgorithmType>::run_automatically)) + ); + // flatbuffersbuilder does not yet exist + m_logFileBufferBuilderP = std::make_shared(); + m_KUKAiiwaStateBufferP = std::make_shared>>(); + #ifdef HAVE_spdlog + loggerP = spdlog::stdout_logger_mt("logs/kukaiiwa_logger.txt"); + #endif // HAVE_spdlog + } - const Params &getParams() { return params_; } + const Params &getParams() { return params_; } + + /// Saves any log files and shuts down the driver + /// Called by the destructor, but since this may take a while + /// a separate funtion is provided so the process can be started in parallel. + void destruct() + { + m_shouldStop = true; + device_driver_workP_.reset(); + if (m_driverThread) + { + m_driverThread->join(); + } - ~KukaFRIdriver() { - device_driver_workP_.reset(); + for(auto saveThreadP : m_saveRecordingThreads) + { + saveThreadP->join(); + } + } - if (driver_threadP) { - device_driver_io_service.stop(); - driver_threadP->join(); + ~KukaFRIdriver() { + destruct(); } - } - /// gets the number of seconds in one message exchange "tick" aka "cycle", - /// "time step" of the robot arm's low level controller - double getSecondsPerTick() { - return std::chrono::duration_cast( - std::chrono::milliseconds(grl::robot::arm::get( - friData_->monitoringMsg, grl::time_step_tag()))) - .count(); - } + /// gets the number of seconds in one message exchange "tick" aka "cycle", + /// "time step" of the robot arm's low level controller + double getSecondsPerTick() { + return std::chrono::duration_cast( + std::chrono::milliseconds(grl::robot::arm::get( + friData_->monitoringMsg, grl::time_step_tag()))).count(); + } - /// @todo make this configurable for different specific robots. Currently set - /// for kuka lbr iiwa 14kg R820 - KukaState::joint_state getMaxVel() { - KukaState::joint_state maxVel; - /// get max velocity constraint parameter for this robot model - copy(std::get(params_), std::back_inserter(maxVel), - grl::revolute_joint_velocity_open_chain_state_constraint_tag()); - - // scale velocity down to a single timestep. In other words multiply each - // velocity by the number of seconds in a tick, likely 0.001-0.005 - boost::transform( - maxVel, maxVel.begin(), - std::bind2nd(std::multiplies(), - getSecondsPerTick())); - - return maxVel; - } + /// @todo make this configurable for different specific robots. Currently set + /// for kuka lbr iiwa 14kg R820 + KukaState::joint_state getMaxVel() { + KukaState::joint_state maxVel; + /// get max velocity constraint parameter for this robot model + copy(std::get(params_), std::back_inserter(maxVel), + grl::revolute_joint_velocity_open_chain_state_constraint_tag()); + + // scale velocity down to a single timestep. In other words multiply each + // velocity by the number of seconds in a tick, likely 0.001-0.005 + boost::transform( maxVel, maxVel.begin(), std::bind2nd(std::multiplies(), + getSecondsPerTick())); + return maxVel; + } - /** - * spin once, this is what you call each time you synchronize the client with - * the robot over UDP - * it is expected to be called at least once per send_period_millisec, which - * is the time between - * each FRI udp packet. - * - */ - bool run_one() { - // note: this one sends *and* receives the joint data! - BOOST_VERIFY(kukaFRIClientDataDriverP_.get() != nullptr); - /// @todo use runtime calculation of NUM_JOINTS instead of constant - if (!friData_) - friData_ = - std::make_shared(KUKA::LBRState::NUM_DOF); - - bool haveNewData = false; - - static const std::size_t minimumConsecutiveSuccessesBeforeSendingCommands = - 100; - - std::shared_ptr lowLevelStepAlgorithmCommandParamsP; - - /// @todo probably only need to set this once - armState.velocity_limits.clear(); - armState.velocity_limits = getMaxVel(); - - // This is the key point where the arm's motion goal command is updated and - // sent to the robot - // Set the FRI to the simulated joint positions - if (this->m_haveReceivedRealDataCount > - minimumConsecutiveSuccessesBeforeSendingCommands) { - /// @todo TODO(ahundt) Need to eliminate this allocation - boost::lock_guard lock(jt_mutex); + struct MicrosecondClock + { + using rep = int64_t; + /// 1 microsecond + using period = std::ratio<1, 1000000>; + using duration = std::chrono::duration; + using time_point = std::chrono::time_point; + + static constexpr bool is_steady = true; + + /// TODO(ahundt) currently assuming the FusionTrack timestamp is from the unix time epoch + static time_point now() noexcept + { + using namespace std::chrono; + return time_point(duration_cast(system_clock::now().time_since_epoch())); + } + }; - boost::container::static_vector jointStateToCommand; - boost::copy(armState.commandedPosition,std::back_inserter(jointStateToCommand)); - // pass time to reach specified goal for position control - lowLevelStepAlgorithmCommandParamsP = std::make_shared(std::make_tuple(jointStateToCommand,armState.goal_position_command_time_duration)); - /// @todo construct new low level command object and pass to - /// KukaFRIClientDataDriver - /// this is where we used to setup a new FRI command - - // std::cout << "commandToSend: " << commandToSend << "\n" << - // "currentJointPos: " << currentJointPos << "\n" << "amountToMove: " << - // amountToMove << "\n" << "maxVel: " << maxvel << "\n"; - } else { - /// @todo TODO(ahundt) BUG: Need way to pass time to reach specified goal for position control and eliminate this allocation - lowLevelStepAlgorithmCommandParamsP.reset(new typename LowLevelStepAlgorithmType::Params()); + /// TODO(ahundt) currently assuming the FusionTrack timestamp is from the unix time epoch + cartographer::common::Time KukaTimeToCommonTime(typename MicrosecondClock::time_point Kukatime) + { + return cartographer::common::Time( + std::chrono::duration_cast(Kukatime.time_since_epoch())); + } + + /// TODO(ahundt) currently assuming the FusionTrack timestamp is from the unix time epoch + cartographer::common::Time KukaTimeToCommonTime( std::chrono::time_point Kukatime) + { + return cartographer::common::Time( + // std::chrono::duration_cast(Kukatime.time_since_epoch())); + std::chrono::duration_cast(Kukatime.time_since_epoch())); } - BOOST_VERIFY(lowLevelStepAlgorithmCommandParamsP != nullptr); - boost::system::error_code send_ec, recv_ec; - std::size_t send_bytes, recv_bytes; - // sync with device over network - haveNewData = !kukaFRIClientDataDriverP_->update_state( - lowLevelStepAlgorithmCommandParamsP, friData_, recv_ec, recv_bytes, send_ec, - send_bytes); - m_attemptedCommunicationCount++; + cartographer::common::Time FRITimeStampToCommonTime(const ::TimeStamp &friTimeStamp) + { + // Convert the time to microseconds - if (haveNewData) { - boost::lock_guard lock(jt_mutex); - // if there were problems sending commands, start by sending the current - // position - // if(this->m_haveReceivedRealDataCount > - // minimumConsecutiveSuccessesBeforeSendingCommands-1) - // { - // boost::lock_guard lock(jt_mutex); - // // initialize arm commands to current arm position - // armState.clearCommands(); - //// armState.commandedPosition.clear(); - //// armState.commandedTorque.clear(); - //// grl::robot::arm::copy(friData_->monitoringMsg, - /// std::back_inserter(armState.commandedPosition), - /// grl::revolute_joint_angle_open_chain_command_tag()); - //// grl::robot::arm::copy(friData_->monitoringMsg, - /// std::back_inserter(armState.commandedTorque) , - /// grl::revolute_joint_torque_open_chain_command_tag()); - // } - - m_attemptedCommunicationConsecutiveSuccessCount++; - this->m_attemptedCommunicationConsecutiveFailureCount = 0; - this->m_haveReceivedRealDataCount++; - - // We have the real kuka state read from the device now - // update real joint angle data - armState.position.clear(); - grl::robot::arm::copy(friData_->monitoringMsg, - std::back_inserter(armState.position), - grl::revolute_joint_angle_open_chain_state_tag()); - - armState.torque.clear(); - grl::robot::arm::copy(friData_->monitoringMsg, - std::back_inserter(armState.torque), - grl::revolute_joint_torque_open_chain_state_tag()); - - armState.externalTorque.clear(); - grl::robot::arm::copy( - friData_->monitoringMsg, std::back_inserter(armState.externalTorque), - grl::revolute_joint_torque_external_open_chain_state_tag()); - -// only supported for kuka sunrise OS 1.9 -#ifdef KUKA_SUNRISE_1_9 - armState.externalForce.clear(); - grl::robot::arm::copy(friData_->monitoringMsg, - std::back_inserter(armState.externalForce), - grl::cartesian_external_force_tag()); -#endif // KUKA_SUNRISE_1_9 - armState.ipoJointPosition.clear(); - grl::robot::arm::copy( - friData_->monitoringMsg, - std::back_inserter(armState.ipoJointPosition), - grl::revolute_joint_angle_interpolated_open_chain_state_tag()); - - armState.sendPeriod = std::chrono::milliseconds( - grl::robot::arm::get(friData_->monitoringMsg, grl::time_step_tag())); - - // std::cout << "Measured Torque: "; - // std::cout << std::setw(6); - // for (float t:armState.torque) { - // std::cout << t << " "; - // } - // std::cout << '\n'; - // - // std::cout << "External Torque: "; - // std::cout << std::setw(6); - // for (float t:armState.externalTorque) { - // std::cout << t << " "; - // } - // std::cout << '\n'; - // - // std::cout << "External Force: "; - // for (float t:armState.externalForce) { - // std::cout << t << " "; - // } - // std::cout << '\n'; - - } else { - m_attemptedCommunicationConsecutiveFailureCount++; - std::cerr << "No new FRI data available, is an FRI application running " - "on the Kuka arm? \n Total sucessful transfers: " - << this->m_haveReceivedRealDataCount - << "\n Total attempts: " << m_attemptedCommunicationCount - << "\n Consecutive Failures: " - << m_attemptedCommunicationConsecutiveFailureCount - << "\n Consecutive Successes: " - << m_attemptedCommunicationConsecutiveSuccessCount << "\n"; - m_attemptedCommunicationConsecutiveSuccessCount = 0; - /// @todo TODO(ahundt) should the results of getlatest state even be possible to call - /// without receiving real data? should the library change? - /// @todo TODO(ahundt) use spdlog library instead of cerr? + std::chrono::time_point timestamp; + timestamp += std::chrono::seconds(friTimeStamp.sec) + std::chrono::nanoseconds(friTimeStamp.nanosec); + + // std::chrono::seconds sec(friTimeStamp.sec); + // int64_t seconds = std::chrono::seconds(sec).count(); + // std::chrono::nanoseconds nanosec(friTimeStamp.nanosec); + // int64_t nanosecs = friTimeStamp.nanosec; + // int64_t microseconds_sec = std::chrono::microseconds(sec).count(); + + + // int64_t microseconds = microseconds + nanosecs/1000; + // if(INT64_MAX - microseconds_sec < nanosecs/1000) { + // throw std::runtime_error("signed overflow has occured"); + // } + // std::cout<<"secondes: "<(KUKA::LBRState::NUM_DOF); + } - /** - * \brief Set the joint positions for the current interpolation step. - * - * This method is only effective when the robot is in a commanding state - * and the set time point for reaching the destination is in the future. - * This function sets the goal time point for a motion to the epoch, aka "time - * 0" (which is in the past) for safety. - * - * - * For position based motion to work, you must set both the position you want - * and the time you want it to get there. - * This is required because the robot can move extremely fast, so accidentally - * setting the velocity to the max is - * very dangerous. If the time point is in the past, the robot will not move. - * If the time point is too near in the future - * to make it, the robot will move at the max speed towards that position. - * - * @see KukaFRIdriver::set(TimePoint && duration_to_goal_command, time_duration_command_tag) to set - * the destination time point in the future so the position motion can start. - * - * @param state Object which stores the current state of the robot, including - * the command to send next - * @param range Array with the new joint positions (in radians) - * @param tag identifier object indicating that revolute joint angle commands - * should be modified - */ - template - void set(Range &&range, grl::revolute_joint_angle_open_chain_command_tag) { - boost::lock_guard lock(jt_mutex); - armState.clearCommands(); - boost::copy(range, std::back_inserter(armState.commandedPosition)); - boost::copy(range, std::back_inserter(armState.commandedPosition_goal)); - } + bool haveNewData = false; + static const std::size_t minimumConsecutiveSuccessesBeforeSendingCommands = 100; + std::shared_ptr lowLevelStepAlgorithmCommandParamsP; + /// @todo probably only need to set this once + armState.velocity_limits.clear(); + armState.velocity_limits = getMaxVel(); + // This is the key point where the arm's motion goal command is updated and sent to the robot + // Set the FRI to the simulated joint positions + // Why set this check? + if (this->m_haveReceivedRealDataCount > minimumConsecutiveSuccessesBeforeSendingCommands) { + /// @todo TODO(ahundt) Need to eliminate this allocation + boost::lock_guard lock(jt_mutex); + boost::container::static_vector jointStateToCommand; + boost::copy(armState.commandedPosition,std::back_inserter(jointStateToCommand)); + // pass time to reach specified goal for position control + lowLevelStepAlgorithmCommandParamsP = std::make_shared(std::make_tuple(jointStateToCommand,armState.goal_position_command_time_duration)); + /// @todo construct new low level command object and pass to + /// KukaFRIClientDataDriver + /// this is where we used to setup a new FRI command + } else { + /// @todo TODO(ahundt) BUG: Need way to pass time to reach specified goal for position control and eliminate this allocation + lowLevelStepAlgorithmCommandParamsP.reset(new typename LowLevelStepAlgorithmType::Params()); + } - /** - * @brief Set the time duration expected between new position commands in ms - * - * The driver will likely be updated every so often, such as every 25ms, and - * the lowest level of the - * driver may update even more frequently, such as every 1ms. By providing as - * accurate an - * estimate between high level updates the low level driver can smooth out the - * motion through - * interpolation (the default), or another algorithm. See - * LowLevelStepAlgorithmType template parameter - * in the KukaFRIdriver class if you want to change out the low level - * algorithm. - * - * @see KukaFRIdriver::get(time_duration_command_tag) - * - * @param duration_to_goal_command std::chrono time format representing the - * time duration between updates - * - */ - void set(double duration_to_goal_command, time_duration_command_tag) { - boost::lock_guard lock(jt_mutex); - armState.goal_position_command_time_duration = duration_to_goal_command; - } + BOOST_VERIFY(lowLevelStepAlgorithmCommandParamsP != nullptr); + boost::system::error_code send_ec, recv_ec; + std::size_t send_bytes, recv_bytes; + // sync with device over network + haveNewData = !kukaFRIClientDataDriverP_->update_state( + lowLevelStepAlgorithmCommandParamsP, + friData_, + recv_ec, + recv_bytes, + send_ec, + send_bytes, + time_event_stamp); + + m_attemptedCommunicationCount++; + + if (haveNewData) { + boost::lock_guard lock(jt_mutex); + + m_attemptedCommunicationConsecutiveSuccessCount++; + this->m_attemptedCommunicationConsecutiveFailureCount = 0; + this->m_haveReceivedRealDataCount++; + + oneKUKAiiwaStateBuffer(); + + saveToDisk(); + + } else { + m_attemptedCommunicationConsecutiveFailureCount++; + // std::cerr << "No new FRI data available, is an FRI application running " + // "on the Kuka arm? \n Total sucessful transfers: " + // << this->m_haveReceivedRealDataCount + // << "\n Total attempts: " << m_attemptedCommunicationCount + // << "\n Consecutive Failures: " + // << m_attemptedCommunicationConsecutiveFailureCount + // << "\n Consecutive Successes: " + // << m_attemptedCommunicationConsecutiveSuccessCount << "\n"; + m_attemptedCommunicationConsecutiveSuccessCount = 0; + /// @todo TODO(ahundt) Add time information from update_state call here for debugging purposes + /// @todo TODO(ahundt) should the results of getlatest state even be possible to call + /// without receiving real data? should the library change? + /// @todo TODO(ahundt) use spdlog library instead of cerr? + } + return haveNewData; + } - /** - * @brief Get the timestamp of the most recent armState - * - * - * - * @see KukaFRIdriver::set(Range&& range, - * grl::revolute_joint_angle_open_chain_command_tag) - * - */ - KukaState::time_point_type get(time_point_tag) { - boost::lock_guard lock(jt_mutex); - return armState.timestamp; - } + /** + * \brief Set the joint positions for the current interpolation step. + * + * This method is only effective when the robot is in a commanding state + * and the set time point for reaching the destination is in the future. + * This function sets the goal time point for a motion to the epoch, aka "time + * 0" (which is in the past) for safety. + * + * + * For position based motion to work, you must set both the position you want + * and the time you want it to get there. + * This is required because the robot can move extremely fast, so accidentally + * setting the velocity to the max is + * very dangerous. If the time point is in the past, the robot will not move. + * If the time point is too near in the future + * to make it, the robot will move at the max speed towards that position. + * + * @see KukaFRIdriver::set(TimePoint && duration_to_goal_command, time_duration_command_tag) to set + * the destination time point in the future so the position motion can start. + * + * @param state Object which stores the current state of the robot, including + * the command to send next + * @param range Array with the new joint positions (in radians) + * @param tag identifier object indicating that revolute joint angle commands + * should be modified + */ + template + void set(Range &&range, grl::revolute_joint_angle_open_chain_command_tag) { + boost::lock_guard lock(jt_mutex); + armState.clearCommands(); + boost::copy(range, std::back_inserter(armState.commandedPosition)); + boost::copy(range, std::back_inserter(armState.commandedPosition_goal)); + } - /** - * \brief Set the applied joint torques for the current interpolation step. - * - * This method is only effective when the client is in a commanding state. - * The ControlMode of the robot has to be joint impedance control mode. The - * Client Command Mode has to be torque. - * - * @param state Object which stores the current state of the robot, including - * the command to send next - * @param torques Array with the applied torque values (in Nm) - * @param tag identifier object indicating that the torqe value command should - * be modified - */ - template - void set(Range &&range, grl::revolute_joint_torque_open_chain_command_tag) { - boost::lock_guard lock(jt_mutex); - armState.clearCommands(); - boost::copy(range, armState.commandedTorque); - } + /** + * @brief Set the time duration expected between new position commands in ms + * + * The driver will likely be updated every so often, such as every 25ms, and + * the lowest level of the + * driver may update even more frequently, such as every 1ms. By providing as + * accurate an + * estimate between high level updates the low level driver can smooth out the + * motion through + * interpolation (the default), or another algorithm. See + * LowLevelStepAlgorithmType template parameter + * in the KukaFRIdriver class if you want to change out the low level + * algorithm. + * + * @see KukaFRIdriver::get(time_duration_command_tag) + * + * @param duration_to_goal_command std::chrono time format representing the + * time duration between updates + * + */ + void set(double duration_to_goal_command, time_duration_command_tag) { + boost::lock_guard lock(jt_mutex); + armState.goal_position_command_time_duration = duration_to_goal_command; + } - /** - * \brief Set the applied wrench vector of the current interpolation step. - * - * The wrench vector consists of: - * [F_x, F_y, F_z, tau_A, tau_B, tau_C] - * - * F ... forces (in N) applied along the Cartesian axes of the - * currently used motion center. - * tau ... torques (in Nm) applied along the orientation angles - * (Euler angles A, B, C) of the currently used motion center. - * - * This method is only effective when the client is in a commanding state. - * The ControlMode of the robot has to be Cartesian impedance control mode. - * The - * Client Command Mode has to be wrench. - * - * @param state object storing the command data that will be sent to the - * physical device - * @param range wrench Applied Cartesian wrench vector, in x, y, z, roll, - * pitch, yaw force measurments. - * @param tag identifier object indicating that the wrench value command - * should be modified - * - * @todo perhaps support some specific more useful data layouts - */ - template - void set(Range &&range, grl::cartesian_wrench_command_tag) { - boost::lock_guard lock(jt_mutex); - armState.clearCommands(); - std::copy(range, armState.commandedCartesianWrenchFeedForward); - } + /** + * @brief Get the timestamp of the most recent armState + * + * + * + * @see KukaFRIdriver::set(Range&& range, + * grl::revolute_joint_angle_open_chain_command_tag) + * + */ + // KukaState::time_point_type get(time_point_tag) { + // boost::lock_guard lock(jt_mutex); + // return armState.timestamp; + // } + + cartographer::common::Time get(time_point_tag) { + boost::lock_guard lock(jt_mutex); + return armState.time_event_stamp.device_time; + } + + /** + * \brief Set the applied joint torques for the current interpolation step. + * + * This method is only effective when the client is in a commanding state. + * The ControlMode of the robot has to be joint impedance control mode. The + * Client Command Mode has to be torque. + * + * @param state Object which stores the current state of the robot, including + * the command to send next + * @param torques Array with the applied torque values (in Nm) + * @param tag identifier object indicating that the torqe value command should + * be modified + */ + template + void set(Range &&range, grl::revolute_joint_torque_open_chain_command_tag) { + boost::lock_guard lock(jt_mutex); + armState.clearCommands(); + boost::copy(range, armState.commandedTorque); + } + + /** + * \brief Set the applied wrench vector of the current interpolation step. + * + * The wrench vector consists of: + * [F_x, F_y, F_z, tau_A, tau_B, tau_C] + * + * F ... forces (in N) applied along the Cartesian axes of the + * currently used motion center. + * tau ... torques (in Nm) applied along the orientation angles + * (Euler angles A, B, C) of the currently used motion center. + * + * This method is only effective when the client is in a commanding state. + * The ControlMode of the robot has to be Cartesian impedance control mode. + * The + * Client Command Mode has to be wrench. + * + * @param state object storing the command data that will be sent to the + * physical device + * @param range wrench Applied Cartesian wrench vector, in x, y, z, roll, + * pitch, yaw force measurments. + * @param tag identifier object indicating that the wrench value command + * should be modified + * + * @todo perhaps support some specific more useful data layouts + */ + template + void set(Range &&range, grl::cartesian_wrench_command_tag) { + boost::lock_guard lock(jt_mutex); + armState.clearCommands(); + std::copy(range, armState.commandedCartesianWrenchFeedForward); + } + + /// @todo should this exist, is it a good design? is it written correctly? + void get(KukaState &state) { + boost::lock_guard lock(jt_mutex); + state = armState; + } + + /// start recording the kuka state data in memory + /// return true on success, false on failure + bool start_recording(int _single_buffer_limit_bytes) + { + + m_isRecording = true; + single_buffer_limit_bytes = _single_buffer_limit_bytes*MegaByte; + std::cout<< "m_isRecording is set to " << m_isRecording << std::endl; + std::cout<< "Kuka single_buffer_limit_bytes: " << _single_buffer_limit_bytes << " MB" << std::endl; + return m_isRecording; + } + /// stop recording the kuka state data in memory + /// return true on success, false on failure + bool stop_recording() + { + m_isRecording = false; + return !m_isRecording; + } + + bool is_recording() + { + return m_isRecording; + } + + + bool oneKUKAiiwaStateBuffer() + { + // We have the real kuka state read from the device now + // update real joint angle data + std::string RobotName = std::string(std::get(params_)); + std:: string destination = std::string(std::get(params_)); + std::string source = std::string(std::get(params_)); + int16_t portOnRemote = std::stoi(std::string(std::get(params_))); + int16_t portOnController = std::stoi(std::string(std::get(params_))); + std::string basename = RobotName; + + // bool max_control_force_stop_ = false; + // std::vector joint_stiffness_(KUKA::LBRState::NUM_DOF, 0); + // std::vector joint_damping_(KUKA::LBRState::NUM_DOF, 0); + // std::vector joint_AccelerationRel_(KUKA::LBRState::NUM_DOF, 0); + // std::vector joint_VelocityRel_(KUKA::LBRState::NUM_DOF, 0); + // bool updateMinimumTrajectoryExecutionTime = false; + // double minimumTrajectoryExecutionTime = 4; + + //Cartesian Impedance Values + // grl::flatbuffer::Vector3d cart_stiffness_trans_ = grl::flatbuffer::Vector3d(0,0,0); + // grl::flatbuffer::EulerRotation cart_stifness_rot_ = grl::flatbuffer::EulerRotation(0,0,0,grl::flatbuffer::EulerOrder::xyz); + // grl::flatbuffer::Vector3d cart_damping_trans_ = grl::flatbuffer::Vector3d(0,0,0); + // grl::flatbuffer::EulerRotation cart_damping_rot_ = grl::flatbuffer::EulerRotation(0,0,0,grl::flatbuffer::EulerOrder::xyz); + // grl::flatbuffer::EulerPose cart_stiffness_ = grl::flatbuffer::EulerPose(cart_stiffness_trans_, cart_stifness_rot_); + // grl::flatbuffer::EulerPose cart_damping_ = grl::flatbuffer::EulerPose(cart_damping_trans_, cart_damping_rot_); + // grl::flatbuffer::EulerPose cart_max_path_deviation_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(0,0,0), grl::flatbuffer::EulerRotation(0,0,0, grl::flatbuffer::EulerOrder::xyz)); + // grl::flatbuffer::EulerPose cart_max_ctrl_vel_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(0,0,0), grl::flatbuffer::EulerRotation(0,0,0, grl::flatbuffer::EulerOrder::xyz)); + // grl::flatbuffer::EulerPose cart_max_ctrl_force_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(0,0,0), grl::flatbuffer::EulerRotation(0.,0.,0., grl::flatbuffer::EulerOrder::xyz)); + // double nullspace_stiffness_ = 0; + // double nullspace_damping_ = 0; + // bool updatePortOnRemote = false; + + // bool updatePortOnController = false; + + // Resolve the data in FRIMonitoringMessage + // Don't match the FRIMessage.pb.h + grl::flatbuffer::ArmState armControlMode = grl::flatbuffer::ArmState::StartArm; + grl::flatbuffer::KUKAiiwaInterface commandInterface = grl::flatbuffer::KUKAiiwaInterface::SmartServo;// KUKAiiwaInterface::SmartServo; + grl::flatbuffer::KUKAiiwaInterface monitorInterface = grl::flatbuffer::KUKAiiwaInterface::FRI; + + FRIMonitoringMessage monitoringMsg = friData_->monitoringMsg; + std::size_t NUM_DOF = grl::robot::arm::get(monitoringMsg, KUKA::LBRState::NUM_DOF);; + ::ControlMode controlMode = grl::robot::arm::get(monitoringMsg, ::ControlMode()); + ::SafetyState safetyState = grl::robot::arm::get(monitoringMsg, ::SafetyState()); + ::OperationMode operationMode = grl::robot::arm::get(monitoringMsg, ::OperationMode()); + ::ConnectionInfo connectionInfo = monitoringMsg.connectionInfo; + ::FRISessionState friSessionState = grl::robot::arm::get(monitoringMsg, ::FRISessionState()); + ::FRIConnectionQuality friConnectionQuality = grl::robot::arm::get(monitoringMsg, ::FRIConnectionQuality()); + ::ClientCommandMode clientCommandMode = grl::robot::arm::get(monitoringMsg, ::ClientCommandMode()); + + ::OverlayType overlayType = grl::robot::arm::get(monitoringMsg, ::OverlayType()); + + ::MessageMonitorData messageMonitorData = monitoringMsg.monitorData; + + std::string s_event_name = RobotName + "/state"; + std::string device_clock_id_str = s_event_name + "/device/clock"; + std::string local_clock_id_str = s_event_name + "/control_computer/clock/steady"; + + TimeEvent::UnsignedCharArray event_name; + std::size_t length = s_event_name.copy(event_name.begin(), std::min(s_event_name.size(), event_name.size())); + event_name[length] = '\0'; + time_event_stamp.event_name = event_name; + + TimeEvent::UnsignedCharArray device_clock_id; + length = device_clock_id_str.copy(device_clock_id.begin(), std::min(device_clock_id_str.size(), device_clock_id.size())); + device_clock_id[length] = '\0'; + time_event_stamp.device_clock_id = device_clock_id; + + TimeEvent::UnsignedCharArray local_clock_name_arr; + length = local_clock_id_str.copy(local_clock_name_arr.begin(), std::min(local_clock_id_str.size(),local_clock_name_arr.size())); + local_clock_name_arr[length] = '\0'; + time_event_stamp.local_clock_id = local_clock_name_arr; + time_event_stamp.device_time = FRITimeStampToCommonTime(messageMonitorData.timestamp); + armState.time_event_stamp = time_event_stamp; + + //std::cout<< time_event_stamp.event_name << std::endl << time_event_stamp.device_clock_id << std::endl << time_event_stamp.local_clock_id <CreateString(basename); + double duration = boost::chrono::high_resolution_clock::now().time_since_epoch().count(); + bool OK = true; + int64_t sequenceNumber = 0; + + copy(monitoringMsg, armState); + + flatbuffers::Offset controlState = grl::toFlatBuffer(*m_logFileBufferBuilderP, basename, sequenceNumber++, duration, armState, armControlMode); + // flatbuffers::Offset setCartesianImpedance = grl::toFlatBuffer(*m_logFileBufferBuilderP, cart_stiffness_, cart_damping_, + // nullspace_stiffness_, nullspace_damping_, cart_max_path_deviation_, cart_max_ctrl_vel_, cart_max_ctrl_force_, max_control_force_stop_); + flatbuffers::Offset setCartesianImpedance = 0; + // flatbuffers::Offset setJointImpedance = grl::toFlatBuffer(*m_logFileBufferBuilderP, joint_stiffness_, joint_damping_); + flatbuffers::Offset setJointImpedance = 0; + // normalized joint accelerations/velocities from 0 to 1 relative to system capabilities + // how to get the acceleration of the robot? There is no acceleration information in KukaState (armState). + flatbuffers::Offset setSmartServo = 0; //grl::toFlatBuffer(*m_logFileBufferBuilderP, joint_AccelerationRel_, joint_VelocityRel_, updateMinimumTrajectoryExecutionTime, minimumTrajectoryExecutionTime); + // FRI configuration parameters + flatbuffers::Offset FRIConfig = 0; //grl::toFlatBuffer(*m_logFileBufferBuilderP, overlayType, connectionInfo, updatePortOnRemote, portOnRemote, updatePortOnController, portOnController); + // std::vector> tools; + // std::vector> processData_vec; + // @TODO(Chunting) Initialize Pose with 0, we ought to calculate it later with ::Transformation; + // Also assign the configuration parameters with random values, we can figure them out later. + // for(int i=0; i linkObject = grl::toFlatBuffer(*m_logFileBufferBuilderP, linkname, parent, pose, inertia); + // auto singleprocessdata = grl::toFlatBuffer( + // *m_logFileBufferBuilderP, + // "dataType"+ std::to_string(i), + // "defaultValue"+ std::to_string(i), + // "displayName"+ std::to_string(i), + // "id"+ std::to_string(i), + // "min"+ std::to_string(i), + // "max"+ std::to_string(i), + // "unit"+ std::to_string(i), + // "value"+ std::to_string(i)); + // tools.push_back(linkObject); + // processData_vec.push_back(singleprocessdata); + // } + // Set the configuration of the Kuka iiwa + // flatbuffers::Offset kukaiiwaArmConfiguration = grl::toFlatBuffer( + // *m_logFileBufferBuilderP, + // RobotName, + // commandInterface, + // monitorInterface, + // clientCommandMode, + // overlayType, + // controlMode, + // setCartesianImpedance, + // setJointImpedance, + // setSmartServo, + // FRIConfig, + // tools, + // processData_vec, + // "currentMotionCenter", + // true); + flatbuffers::Offset kukaiiwaArmConfiguration; + + flatbuffers::Offset friMessageLog = grl::toFlatBuffer( + *m_logFileBufferBuilderP, + friSessionState, + friConnectionQuality, + controlMode, + monitoringMsg, + armState.time_event_stamp); + // getWrench() is availble in KukaJAVAdriver, so maybe it's better to log data in KukaDriver where user can access both KukaFRIDriver and KukaJAVADriver? + // grl::flatbuffer::Wrench cartesianWrench{grl::flatbuffer::Vector3d(0, 0, 0),grl::flatbuffer::Vector3d(0, 0, 0),grl::flatbuffer::Vector3d(0, 0, 0)}; + grl::flatbuffer::Wrench cartesianWrench; + // In armState there is neither joint velocity nor acceleration + std::vector position; + std::vector velocity; + std::vector acceleration; + std::vector torque; + std::vector jointIpoPostion; + std::vector externalTorque; + for(int i = 0; i i){ + jointIpoPostion.push_back(armState.ipoJointPosition[i]); + } + externalTorque.push_back(armState.externalTorque[i]); + } + flatbuffers::Offset jointStatetab = grl::toFlatBuffer(*m_logFileBufferBuilderP, position, velocity, acceleration, torque); + torque.clear(); + flatbuffers::Offset jointIpoState = grl::toFlatBuffer(*m_logFileBufferBuilderP, jointIpoPostion, velocity, acceleration, torque); + position.clear(); + flatbuffers::Offset externalState = grl::toFlatBuffer(*m_logFileBufferBuilderP, position, velocity, acceleration, externalTorque); + // Calculate the data later. + // Cartesian pose of the flange relative to the base of the arm + // grl::flatbuffer::Pose cartesianFlangePose = grl::flatbuffer::Pose(grl::flatbuffer::Vector3d(0, 0, 0), grl::flatbuffer::Quaternion(0,0,0,0)); + grl::flatbuffer::Pose cartesianFlangePose = grl::flatbuffer::Pose(); + // flatbuffers::Offset kukaiiwaMonitorState = 0; + flatbuffers::Offset kukaiiwaMonitorState = grl::toFlatBuffer( + *m_logFileBufferBuilderP, + jointStatetab, // flatbuffers::Offset &measuredState, + cartesianFlangePose, + jointStatetab, // flatbuffers::Offset &jointStateReal, + jointIpoState, // flatbuffers::Offset &jointStateInterpolated, + externalState, // flatbuffers::Offset &externalState, + friSessionState, + operationMode, + cartesianWrench); + // Set it up in the configuration file + // std::vector torqueSensorLimits(KUKA::LBRState::NUM_DOF,0.1); + // std::string hardwareVersion("hardvareVersion"); + // bool isReadyToMove = true; + // bool isMastered = true; + // flatbuffers::Offset monitorConfig = grl::toFlatBuffer( + // *m_logFileBufferBuilderP, + // hardwareVersion, + // torqueSensorLimits, + // isReadyToMove, + // isMastered, + // processData_vec); + flatbuffers::Offset monitorConfig = 0; + flatbuffers::Offset KUKAiiwaState = grl::toFlatBuffer( + *m_logFileBufferBuilderP, + RobotName, + destination, + source, + armState.time_event_stamp, + false, controlState, // if false, then don't record the value + false, kukaiiwaArmConfiguration, + false, kukaiiwaMonitorState, + false, monitorConfig, + friMessageLog); + m_KUKAiiwaStateBufferP->push_back(KUKAiiwaState); + + return true; + } + + bool save_recording(std::string filename = std::string()) + { + if(filename.empty()) + { + std::string homePath = std::getenv("HOME"); + std::string vrepDataPath = homePath + "/src/V-REP_PRO_EDU_V3_4_0_Linux/data/"; + /// TODO(ahundt) Saving the file twice in one second will overwrite!!!! + filename = vrepDataPath + current_date_and_time_string() + "_Kukaiiwa.iiwa"; + } + #ifdef HAVE_spdlog + loggerP->info("Save Recording as in Kuka: {}", filename); + #else // HAVE_spdlog + std::cout << "Save Recording as: " << filename << std::endl; + #endif // HAVE_spdlog + auto saveLambdaFunction = [ + save_fbbP = std::move(m_logFileBufferBuilderP) + ,save_KUKAiiwaBufferP = std::move(m_KUKAiiwaStateBufferP) + ,filename + #ifdef HAVE_spdlog + ,lambdaLoggerP = loggerP + #endif // HAVE_spdlog + ]() mutable + { + + std::string currentWorkingDir = boost::filesystem::current_path().string(); + lambdaLoggerP->info("currentWorkingDir ...: {}", currentWorkingDir); + if(save_fbbP != nullptr && save_KUKAiiwaBufferP != nullptr) { + bool success = grl::FinishAndVerifyBuffer(*save_fbbP, *save_KUKAiiwaBufferP); + bool write_binary_stream = true; + success = success && flatbuffers::SaveFile(filename.c_str(), reinterpret_cast(save_fbbP->GetBufferPointer()), save_fbbP->GetSize(), write_binary_stream); + // assert(success); + /// TODO(ahFusionTrackLogAndTrackundt) replace cout with proper spdlog and vrep banner notification + #ifdef HAVE_spdlog + lambdaLoggerP->info("For KUKA filename: {}, verifier success:{}", filename,success); + #else // HAVE_spdlog + std::cout << "filename: " << filename << " verifier success: " << success << std::endl; + #endif // HAVE_spdlog + }else{ + lambdaLoggerP->error("pointer is nullptr..."); + } + }; + + // save the recording to a file in a separate thread, memory will be freed up when file finishes saving + std::shared_ptr saveLogThread(std::make_shared(saveLambdaFunction)); + m_saveRecordingThreads.push_back(saveLogThread); + // flatbuffersbuilder does not yet exist + m_logFileBufferBuilderP = std::make_shared(); + m_KUKAiiwaStateBufferP = std::make_shared>>(); + + return true; + } - /// @todo should this exist, is it a good design? is it written correctly? - void get(KukaState &state) { - boost::lock_guard lock(jt_mutex); - state = armState; - } - // The total number of times the FRI interface has successfully received a UDP - // packet - // from the robot since this class was initialized. - volatile std::size_t m_haveReceivedRealDataCount = 0; - // The total number of times the FRI interface has attempted to receive a UDP - // packet - // from the robot since this class was initialized, regardless of if it was - // successful or not. - volatile std::size_t m_attemptedCommunicationCount = 0; - // The number of consecutive FRI receive calls that have failed to get data - // successfully, resets to 0 on a single success. - volatile std::size_t m_attemptedCommunicationConsecutiveFailureCount = 0; - // The number of consecutive FRI receive calls that have received data - // successfully, resets to 0 on a single failure. - volatile std::size_t m_attemptedCommunicationConsecutiveSuccessCount = 0; - - boost::asio::io_service device_driver_io_service; - std::unique_ptr device_driver_workP_; - std::unique_ptr driver_threadP; - std::shared_ptr< - grl::robot::arm::KukaFRIClientDataDriver> - kukaFRIClientDataDriverP_; + // clear the recording buffer from memory immediately to start fresh + void clear_recording() + { + boost::lock_guard lock(jt_mutex); + m_logFileBufferBuilderP.reset(); + m_KUKAiiwaStateBufferP.reset(); + } + + // The total number of times the FRI interface has successfully received a UDP + // packet + // from the robot since this class was initialized. + volatile std::size_t m_haveReceivedRealDataCount = 0; + // The total number of times the FRI interface has attempted to receive a UDP + // packet + // from the robot since this class was initialized, regardless of if it was + // successful or not. + volatile std::size_t m_attemptedCommunicationCount = 0; + // The number of consecutive FRI receive calls that have failed to get data + // successfully, resets to 0 on a single success. + volatile std::size_t m_attemptedCommunicationConsecutiveFailureCount = 0; + // The number of consecutive FRI receive calls that have received data + // successfully, resets to 0 on a single failure. + volatile std::size_t m_attemptedCommunicationConsecutiveSuccessCount = 0; + + boost::asio::io_service device_driver_io_service; + // The work class is used to inform the io_service when work starts and finishes. + std::unique_ptr device_driver_workP_; + std::unique_ptr driver_threadP; + std::shared_ptr> kukaFRIClientDataDriverP_; private: - KukaState armState; - boost::mutex jt_mutex; +/// Check the size of the buffer, when it hit the limit, save it to disk. +void saveToDisk() + { + + + // run the primary update loop in a separate thread + bool saveFileNow = false; + /// Temporarily set m_isRecording true. + /// m_isRecording = true; + if (m_isRecording) + { + // There is a flatbuffers file size limit of 2GB, but we use a conservative 512MB + int buffsize = m_logFileBufferBuilderP->GetSize(); + int statessize = m_KUKAiiwaStateBufferP->size(); + // if( buffsize > single_buffer_limit_bytes || statessize > single_buffer_limit_states) + if( buffsize > single_buffer_limit_bytes) + { + // save the file if we are over the limit + saveFileNow = true; + std::cout << "Buffersize:" << buffsize << std::endl; + } + } // end recording steps + /// TODO(ahundt) Let the user specify the filenames, or provide a way to check the flatbuffer size and know single_buffer_limit_bytes. + if(saveFileNow) + { + save_recording(); + saveFileNow = false; + } - Params params_; - std::shared_ptr friData_; -}; + } +private: + KukaState armState; + boost::mutex jt_mutex; + + Params params_; + std::shared_ptr friData_; + std::atomic m_shouldStop; + /// is data currently being recorded + std::atomic m_isRecording; + /// builds up the file log in memory as data is received + /// @todo TODO(ahundt) once using C++14 use unique_ptr https://stackoverflow.com/questions/8640393/move-capture-in-lambda + std::shared_ptr m_logFileBufferBuilderP; + /// this is the current log data stored in memory + /// @todo TODO(ahundt) once using C++14 use unique_ptr https://stackoverflow.com/questions/8640393/move-capture-in-lambda + std::shared_ptr>> m_KUKAiiwaStateBufferP; + /// thread that polls the driver for new data and puts the data into the recording + std::unique_ptr m_driverThread; + /// @todo TODO(ahundt) the threads that saved files will build up forever, figure out how they can clear themselves out + std::vector> m_saveRecordingThreads; + + grl::TimeEvent time_event_stamp; + + const std::size_t MegaByte = 1024*1024; + // If we write too large a flatbuffer + std::size_t single_buffer_limit_bytes = 20*MegaByte; + const std::size_t single_buffer_limit_states = 1350000000; + + #ifdef HAVE_spdlog + std::shared_ptr loggerP; + #endif // HAVE_spdlog +}; /// End of KukaFRIdriver.hpp /// @brief nonmember wrapper function to help integrate KukaFRIdriver objects /// with generic programming interface diff --git a/include/grl/kuka/KukaJAVAdriver.hpp b/include/grl/kuka/KukaJAVAdriver.hpp index 38d8349..6472d78 100644 --- a/include/grl/kuka/KukaJAVAdriver.hpp +++ b/include/grl/kuka/KukaJAVAdriver.hpp @@ -7,10 +7,10 @@ #include #include #include +#include // Required #include #include -#include #include #include #include @@ -30,6 +30,12 @@ #include #include #include +// C++11, use std::false_type and std::true_type +#include +#include + +#include // Required +#include // Required //#ifdef BOOST_NO_CXX11_ATOMIC_SMART_PTR #include @@ -47,6 +53,7 @@ #include #include +#include namespace grl { namespace robot { namespace arm { @@ -61,410 +68,382 @@ namespace grl { namespace robot { namespace arm { * @todo make sure mutex is locked when appropriate * */ - class KukaJAVAdriver : public std::enable_shared_from_this { +class KukaJAVAdriver : public std::enable_shared_from_this { public: - enum ParamIndex { - RobotName, - RobotModel, - LocalUDPAddress, - LocalUDPPort, - RemoteUDPAddress, - LocalHostKukaKoniUDPAddress, - LocalHostKukaKoniUDPPort, - RemoteHostKukaKoniUDPAddress, - RemoteHostKukaKoniUDPPort, - KukaCommandMode, - KukaMonitorMode - }; - - typedef std::tuple< - std::string, - std::string, - std::string, - std::string, - std::string, - std::string, - std::string, - std::string, - std::string, - std::string, - std::string - > Params; - - - static const Params defaultParams(){ - return std::make_tuple( - "Robotiiwa" , // RobotName, - "KUKA_LBR_IIWA_14_R820" , // RobotModel (options are KUKA_LBR_IIWA_14_R820, KUKA_LBR_IIWA_7_R800) - "0.0.0.0" , // LocalUDPAddress - "30010" , // LocalUDPPort - "172.31.1.147" , // RemoteUDPAddress - "192.170.10.100" , // LocalHostKukaKoniUDPAddress, - "30200" , // LocalHostKukaKoniUDPPort, - "192.170.10.2" , // RemoteHostKukaKoniUDPAddress, - "30200" , // RemoteHostKukaKoniUDPPort - "JAVA" , // KukaCommandMode (options are FRI, JAVA) - "FRI" // KukaMonitorMode (options are FRI, JAVA) - ); - } - - - /// unique tag type so State never - /// conflicts with a similar tuple - struct JointStateTag{}; - - enum JointStateIndex { - JointPosition, - JointForce, - JointTargetPosition, - JointLowerPositionLimit, - JointUpperPositionLimit, - JointMatrix, - JointStateTagIndex - }; - - - typedef std::vector JointScalar; - - /// @see http://www.coppeliarobotics.com/helpFiles/en/apiFunctions.htm#simGetJointMatrix for data layout information - typedef std::array TransformationMatrix; - typedef std::vector TransformationMatrices; - - typedef std::tuple< - JointScalar, // jointPosition - // JointScalar // JointVelocity // no velocity yet - JointScalar, // jointForce - JointScalar, // jointTargetPosition - JointScalar, // JointLowerPositionLimit - JointScalar, // JointUpperPositionLimit - TransformationMatrices, // jointTransformation - JointStateTag // JointStateTag unique identifying type so tuple doesn't conflict - > State; - - - KukaJAVAdriver(Params params = defaultParams()) - : params_(params), armControlMode_(flatbuffer::ArmState::NONE) - {} - - void construct(){ construct(params_); sequenceNumber = 0; } - - /// @todo create a function that calls simGetObjectHandle and throws an exception when it fails - /// @warning getting the ik group is optional, so it does not throw an exception - void construct(Params params) { - try { logger_ = spdlog::stdout_logger_mt("console"); } catch (spdlog::spdlog_ex ex) { logger_ = spdlog::get("console"); } - params_ = params; - - - try { - logger_->info("KukaLBRiiwaRosPlugin: Connecting UDP Socket from ", - std::get (params_), ":", std::get (params_), " to ", - std::get (params_)); - - /// @todo TODO(ahundt) switch from linux socket to boost::asio::ip::udp::socket, see Kuka.hpp and KukaFRIdriver.hpp for examples, and make use of KukaUDP class. - socket_local = socket(AF_INET, SOCK_DGRAM, 0); - if (socket_local < 0) { - BOOST_THROW_EXCEPTION(std::runtime_error("KukaJAVAdriver Error opening socket. Check that the port is available, and that all the cables are connected tightly. If you have no other options try restarting your computer.")); - } - - port = boost::lexical_cast( std::get (params_)); - // convert the string to network presentation value - inet_pton(AF_INET, std::get(params_).c_str(), &(local_sockaddr.sin_addr)); - local_sockaddr.sin_family = AF_INET; - local_sockaddr.sin_port = htons(port); - // local_sockaddr.sin_addr.s_addr = INADDR_ANY; - - /// @todo TODO(ahundt) Consider switching to boost::asio synchronous calls (async has high latency)! - /// @todo TODO(ahundt) Need to switch back to an appropriate exception rather than exiting so VREP isn't taken down. - /// @todo TODO(ahundt) switch from linux socket to boost::asio::ip::udp::socket, see Kuka.hpp and KukaFRIdriver.hpp for examples, and make use of KukaUDP class. - if (bind(socket_local, (struct sockaddr *)&local_sockaddr, sizeof(local_sockaddr)) < 0) { - printf("Error binding sr_joint!\n"); - BOOST_THROW_EXCEPTION(std::runtime_error("KukaJAVAdriver Error opening socket. Check that the port is available, and that all the cables are connected tightly. If you have no other options try restarting your computer.")); - } - - FD_ZERO(&mask); - FD_ZERO(&dummy_mask); - FD_SET(socket_local, &mask); - - // set arm to StartArm mode on initalization - //set(grl::flatbuffer::ArmState::StartArm); - set(grl::flatbuffer::ArmState::MoveArmJointServo); - - } catch( boost::exception &e) { - e << errmsg_info("KukaLBRiiwaRosPlugin: Unable to connect to UDP Socket from {}{}{}" + - std::get (params_) + " to " + - std::get (params_)); - throw; + enum ParamIndex { + RobotName, + RobotModel, + LocalUDPAddress, + LocalUDPPort, + RemoteUDPAddress, + LocalHostKukaKoniUDPAddress, + LocalHostKukaKoniUDPPort, + RemoteHostKukaKoniUDPAddress, + RemoteHostKukaKoniUDPPort, + KukaCommandMode, + KukaMonitorMode + }; + + typedef std::tuple< + std::string, + std::string, + std::string, + std::string, + std::string, + std::string, + std::string, + std::string, + std::string, + std::string, + std::string + > Params; + + + static const Params defaultParams(){ + return std::make_tuple( + "Robotiiwa" , // RobotName, + "KUKA_LBR_IIWA_14_R820" , // RobotModel (options are KUKA_LBR_IIWA_14_R820, KUKA_LBR_IIWA_7_R800) + "0.0.0.0" , // LocalUDPAddress + "30010" , // LocalUDPPort + "172.31.1.147" , // RemoteUDPAddress + "192.170.10.100" , // LocalHostKukaKoniUDPAddress, + "30200" , // LocalHostKukaKoniUDPPort, + "192.170.10.2" , // RemoteHostKukaKoniUDPAddress, + "30200" , // RemoteHostKukaKoniUDPPort + "JAVA" , // KukaCommandMode (options are FRI, JAVA) + "FRI" // KukaMonitorMode (options are FRI, JAVA) + ); } - } - - - - - const Params & getParams(){ - return params_; - } - - /// shuts down the arm - bool destruct(){ - close(socket_local); - return true; - } - ~KukaJAVAdriver(){ - /// @todo TODO(ahundt) switch to asio, remove destruct call from destructor - destruct(); - } + /// unique tag type so State never + /// conflicts with a similar tuple + struct JointStateTag{}; + + enum JointStateIndex { + JointPosition, + JointForce, + JointTargetPosition, + JointLowerPositionLimit, + JointUpperPositionLimit, + JointMatrix, + JointStateTagIndex + }; + + typedef std::vector JointScalar; + + /// @see http://www.coppeliarobotics.com/helpFiles/en/apiFunctions.htm#simGetJointMatrix for data layout information + typedef std::array TransformationMatrix; + typedef std::vector TransformationMatrices; + + typedef std::tuple< + JointScalar, // jointPosition + // JointScalar // JointVelocity // no velocity yet + JointScalar, // jointForce + JointScalar, // jointTargetPosition + JointScalar, // JointLowerPositionLimit + JointScalar, // JointUpperPositionLimit + TransformationMatrices, // jointTransformation + JointStateTag // JointStateTag unique identifying type so tuple doesn't conflict + > State; + + + KukaJAVAdriver(Params params = defaultParams()) + : params_(params), armControlMode_(flatbuffer::ArmState::NONE) + {} + + void construct(){ construct(params_); sequenceNumber = 0; } + + /// @todo create a function that calls simGetObjectHandle and throws an exception when it fails + /// @warning getting the ik group is optional, so it does not throw an exception + void construct(Params params) { + try { logger_ = spdlog::stdout_logger_mt("console"); } catch (spdlog::spdlog_ex ex) { logger_ = spdlog::get("console"); } + params_ = params; + + try { + logger_->info("KukaLBRiiwaRosPlugin: Connecting UDP Socket from ", + std::get (params_), ":", std::get (params_), " to ", + std::get (params_)); + + /// @todo TODO(ahundt) switch from linux socket to boost::asio::ip::udp::socket, see Kuka.hpp and KukaFRIdriver.hpp for examples, and make use of KukaUDP class. + /// AF_INET (IPv4 protocol) , AF_INET6 (IPv6 protocol) + /// communication type: SOCK_STREAM: TCP(reliable, connection oriented), SOCK_DGRAM: UDP(unreliable, connectionless) + /// Protocol value for Internet Protocol(IP), which is 0. + /// socket_local is the socket descriptor, which is an integer. + socket_local = socket(AF_INET, SOCK_DGRAM, 0); + if (socket_local < 0) { + BOOST_THROW_EXCEPTION(std::runtime_error("KukaJAVAdriver Error opening socket. Check that the port is available, and that all the cables are connected tightly. If you have no other options try restarting your computer.")); + } + /// convert string to int + port = boost::lexical_cast( std::get (params_)); + /// convert the string to network presentation value + /// inet_pton() returns 1 on success. It returns -1 if there was an error (errno is set), or 0 if the input isn't a valid IP address. + inet_pton(AF_INET, std::get(params_).c_str(), &(local_sockaddr.sin_addr)); + local_sockaddr.sin_family = AF_INET; + /// htons: host to network short + local_sockaddr.sin_port = htons(port); + /// local_sockaddr.sin_addr.s_addr = INADDR_ANY; + /// @todo TODO(ahundt) Consider switching to boost::asio synchronous calls (async has high latency)! + /// @todo TODO(ahundt) Need to switch back to an appropriate exception rather than exiting so VREP isn't taken down. + /// @todo TODO(ahundt) switch from linux socket to boost::asio::ip::udp::socket, see Kuka.hpp and KukaFRIdriver.hpp for examples, and make use of KukaUDP class. + /// After creation of the socket, bind function binds the socket to the address and port number specified in local_sockaddr(custom data structure). + if (::bind(socket_local, (struct sockaddr *)&local_sockaddr, sizeof(local_sockaddr)) < 0) { + printf("Error binding sr_joint!\n"); + BOOST_THROW_EXCEPTION(std::runtime_error("KukaJAVAdriver Error opening socket. Check that the port is available, and that all the cables are connected tightly. If you have no other options try restarting your computer.")); + } + /// clear the socket set defined by fd_set + FD_ZERO(&mask); + FD_ZERO(&dummy_mask); + /// if valid socket descriptor then add to socket set + FD_SET(socket_local, &mask); + /// set arm to StartArm mode on initalization + /// set(grl::flatbuffer::ArmState::StartArm); + set(grl::flatbuffer::ArmState::MoveArmJointServo); + } catch( boost::exception &e) { + e << errmsg_info("KukaLBRiiwaRosPlugin: Unable to connect to UDP Socket from {}{}{}" + + std::get (params_) + " to " + + std::get (params_)); + throw; + } + } - /// @brief SEND COMMAND TO ARM. Call this often - /// Performs the main update spin once. - /// @todo ADD SUPPORT FOR READING ARM STATE OVER JAVA INTERFACE - bool run_one(){ - - // @todo CHECK FOR REAL DATA BEFORE SENDING COMMANDS - //if(!m_haveReceivedRealDataCount) return; - - bool haveNewData = false; - - /// @todo make this handled by template driver implementations/extensions + const Params & getParams(){ + return params_; + } + /// shuts down the arm + bool destruct(){ + close(socket_local); + return true; + } + ~KukaJAVAdriver(){ + /// @todo TODO(ahundt) switch to asio, remove destruct call from destructor + destruct(); + } + /// @brief SEND COMMAND TO ARM. Call this often + /// Performs the main update spin once. + /// @todo ADD SUPPORT FOR READING ARM STATE OVER JAVA INTERFACE + bool run_one(){ + // @todo CHECK FOR REAL DATA BEFORE SENDING COMMANDS + //if(!m_haveReceivedRealDataCount) return; + bool haveNewData = false; + /// @todo make this handled by template driver implementations/extensions - std::shared_ptr fbbP; - fbbP = std::make_shared(); + std::shared_ptr fbbP; + fbbP = std::make_shared(); boost::lock_guard lock(jt_mutex); - double duration = boost::chrono::high_resolution_clock::now().time_since_epoch().count(); - - /// @todo is this the best string to pass for the full arm's name? - auto basename = std::get(params_); - - auto bns = fbbP->CreateString(basename); - - flatbuffers::Offset controlState; + double duration = boost::chrono::high_resolution_clock::now().time_since_epoch().count(); + /// @todo is this the best string to pass for the full arm's name? + auto basename = std::get(params_); - switch (armControlMode_) { + auto bns = fbbP->CreateString(basename); - case flatbuffer::ArmState::StartArm: { - controlState = flatbuffer::CreateArmControlState(*fbbP,bns,sequenceNumber++,duration,armControlMode_,flatbuffer::CreateStartArm(*fbbP).Union()); - break; - } - case flatbuffer::ArmState::MoveArmJointServo: { + flatbuffers::Offset controlState; - /// @todo when new - auto armPositionBuffer = fbbP->CreateVector(armState_.commandedPosition_goal.data(),armState_.commandedPosition_goal.size()); - auto commandedTorque = fbbP->CreateVector(armState_.commandedTorque.data(),armState_.commandedTorque.size()); - auto goalJointState = grl::flatbuffer::CreateJointState(*fbbP,armPositionBuffer,0/*no velocity*/,0/*no acceleration*/,commandedTorque); - auto moveArmJointServo = grl::flatbuffer::CreateMoveArmJointServo(*fbbP,goalJointState); - controlState = flatbuffer::CreateArmControlState(*fbbP,bns,sequenceNumber++,duration,armControlMode_,moveArmJointServo.Union()); - logger_->info("C++ KukaJAVAdriver: sending armposition command: {}{}", armState_.commandedPosition_goal); - break; - } - case flatbuffer::ArmState::TeachArm: { - controlState = flatbuffer::CreateArmControlState(*fbbP,bns,sequenceNumber++,duration,armControlMode_,flatbuffer::CreateTeachArm(*fbbP).Union()); - break; - } - case flatbuffer::ArmState::PauseArm: { - controlState = flatbuffer::CreateArmControlState(*fbbP,bns,sequenceNumber++,duration,armControlMode_,flatbuffer::CreatePauseArm(*fbbP).Union()); - break; - } - case flatbuffer::ArmState::StopArm: { - controlState = flatbuffer::CreateArmControlState(*fbbP,bns,sequenceNumber++,duration,armControlMode_,flatbuffer::CreateStopArm(*fbbP).Union()); - break; - } - case flatbuffer::ArmState::ShutdownArm: { - controlState = flatbuffer::CreateArmControlState(*fbbP,bns,sequenceNumber++,duration,armControlMode_,flatbuffer::CreateStopArm(*fbbP).Union()); - break; - } - case flatbuffer::ArmState::NONE: { - //std::cerr << "Waiting for interation mode... (currently NONE)\n"; - break; - } - default: - logger_->error("C++ KukaJAVAdriver: unsupported use case: {}", EnumNameArmState(armControlMode_)); - } + switch (armControlMode_) { - auto name = fbbP->CreateString(std::get(params_)); - - auto clientCommandMode = grl::flatbuffer::EClientCommandMode::POSITION; - auto overlayType = grl::flatbuffer::EOverlayType::NO_OVERLAY; + case flatbuffer::ArmState::StartArm: { + controlState = flatbuffer::CreateArmControlState(*fbbP,bns,sequenceNumber++,duration,armControlMode_,flatbuffer::CreateStartArm(*fbbP).Union()); + break; + } + case flatbuffer::ArmState::MoveArmJointServo: { + + /// @todo when new + /// armState_ isn't assigned with any values correctly. + auto armPositionBuffer = fbbP->CreateVector(armState_.commandedPosition_goal.data(),armState_.commandedPosition_goal.size()); + auto commandedTorque = fbbP->CreateVector(armState_.commandedTorque.data(),armState_.commandedTorque.size()); + auto goalJointState = grl::flatbuffer::CreateJointState(*fbbP,armPositionBuffer,0/*no velocity*/,0/*no acceleration*/,commandedTorque); + auto moveArmJointServo = grl::flatbuffer::CreateMoveArmJointServo(*fbbP,goalJointState); + controlState = flatbuffer::CreateArmControlState(*fbbP,bns,sequenceNumber++,duration,armControlMode_,moveArmJointServo.Union()); + logger_->info("C++ KukaJAVAdriver: sending armposition command: {}{}", armState_.commandedPosition_goal); + break; + } + case flatbuffer::ArmState::TeachArm: { + controlState = flatbuffer::CreateArmControlState(*fbbP,bns,sequenceNumber++,duration,armControlMode_,flatbuffer::CreateTeachArm(*fbbP).Union()); + break; + } + case flatbuffer::ArmState::PauseArm: { + controlState = flatbuffer::CreateArmControlState(*fbbP,bns,sequenceNumber++,duration,armControlMode_,flatbuffer::CreatePauseArm(*fbbP).Union()); + break; + } + case flatbuffer::ArmState::StopArm: { + controlState = flatbuffer::CreateArmControlState(*fbbP,bns,sequenceNumber++,duration,armControlMode_,flatbuffer::CreateStopArm(*fbbP).Union()); + break; + } + case flatbuffer::ArmState::ShutdownArm: { + controlState = flatbuffer::CreateArmControlState(*fbbP,bns,sequenceNumber++,duration,armControlMode_,flatbuffer::CreateStopArm(*fbbP).Union()); + break; + } + case flatbuffer::ArmState::NONE: { + //std::cerr << "Waiting for interation mode... (currently NONE)\n"; + break; + } + default: + logger_->error("C++ KukaJAVAdriver: unsupported use case: {}", EnumNameArmState(armControlMode_)); + } - //auto stiffnessPose = flatbuffer::CreateEulerPoseParams(*fbbP,&cart_stiffness_trans_,&cart_stiffness_rot_); - //auto dampingPose = flatbuffer::CreateEulerPoseParams(*fbbP,&cart_damping_trans_,&cart_damping_rot_); + auto name = fbbP->CreateString(std::get(params_)); - auto setCartesianImpedance = grl::flatbuffer::CreateCartesianImpedenceControlMode(*fbbP, &cart_stiffness_, &cart_damping_, - nullspace_stiffness_, nullspace_damping_, &cart_max_path_deviation_, &cart_max_ctrl_vel_, &cart_max_ctrl_force_, max_control_force_stop_); + auto clientCommandMode = grl::flatbuffer::EClientCommandMode::POSITION; + auto overlayType = grl::flatbuffer::EOverlayType::NO_OVERLAY; - auto jointStiffnessBuffer = fbbP->CreateVector(joint_stiffness_.data(),joint_stiffness_.size()); - auto jointDampingBuffer = fbbP->CreateVector(joint_damping_.data(),joint_damping_.size()); + //auto stiffnessPose = flatbuffer::CreateEulerPoseParams(*fbbP,&cart_stiffness_trans_,&cart_stiffness_rot_); + //auto dampingPose = flatbuffer::CreateEulerPoseParams(*fbbP,&cart_damping_trans_,&cart_damping_rot_); - auto setJointImpedance = grl::flatbuffer::CreateJointImpedenceControlMode(*fbbP, jointStiffnessBuffer, jointDampingBuffer); + auto setCartesianImpedance = grl::flatbuffer::CreateCartesianImpedenceControlMode(*fbbP, &cart_stiffness_, &cart_damping_, + nullspace_stiffness_, nullspace_damping_, &cart_max_path_deviation_, &cart_max_ctrl_vel_, &cart_max_ctrl_force_, max_control_force_stop_); - auto kukaiiwaArmConfiguration = flatbuffer::CreateKUKAiiwaArmConfiguration(*fbbP,name,commandInterface_,monitorInterface_, clientCommandMode, overlayType, - controlMode_, setCartesianImpedance, setJointImpedance); + auto jointStiffnessBuffer = fbbP->CreateVector(joint_stiffness_.data(),joint_stiffness_.size()); + auto jointDampingBuffer = fbbP->CreateVector(joint_damping_.data(),joint_damping_.size()); - bool setArmControlState = true; // only actually change the arm state when this is true. + auto setJointImpedance = grl::flatbuffer::CreateJointImpedenceControlMode(*fbbP, jointStiffnessBuffer, jointDampingBuffer); - // TODO fill the 0s - auto kukaiiwastate = flatbuffer::CreateKUKAiiwaState(*fbbP,0,0,0,0,1,controlState,setArmConfiguration_,kukaiiwaArmConfiguration); + auto kukaiiwaArmConfiguration = flatbuffer::CreateKUKAiiwaArmConfiguration(*fbbP,name,commandInterface_,monitorInterface_, clientCommandMode, overlayType, + controlMode_, setCartesianImpedance, setJointImpedance); - auto kukaiiwaStateVec = fbbP->CreateVector(&kukaiiwastate, 1); + bool setArmControlState = true; // only actually change the arm state when this is true. - auto states = flatbuffer::CreateKUKAiiwaStates(*fbbP,kukaiiwaStateVec); + // TODO fill the 0s + auto kukaiiwastate = flatbuffer::CreateKUKAiiwaState(*fbbP,0,0,0,0,1,controlState,setArmConfiguration_,kukaiiwaArmConfiguration); - grl::flatbuffer::FinishKUKAiiwaStatesBuffer(*fbbP, states); + auto kukaiiwaStateVec = fbbP->CreateVector(&kukaiiwastate, 1); - flatbuffers::Verifier verifier(fbbP->GetBufferPointer(),fbbP->GetSize()); - BOOST_VERIFY(grl::flatbuffer::VerifyKUKAiiwaStatesBuffer(verifier)); + auto states = flatbuffer::CreateKUKAiiwaStates(*fbbP,kukaiiwaStateVec); + grl::flatbuffer::FinishKUKAiiwaStatesBuffer(*fbbP, states); - if(armControlMode_ == flatbuffer::ArmState::MoveArmJointServo) - { - auto states2 = flatbuffer::GetKUKAiiwaStates(fbbP->GetBufferPointer()); - auto movearm = static_cast(states2->states()->Get(0)->armControlState()->state()); - std::vector angles; - for(std::size_t i = 0; i < movearm->goal()->position()->size(); ++i) - { - angles.push_back(movearm->goal()->position()->Get(i)); - } - logger_->info("re-extracted {}{}{}", movearm->goal()->position()->size(), " joint angles: ",angles); - } + flatbuffers::Verifier verifier(fbbP->GetBufferPointer(),fbbP->GetSize()); + BOOST_VERIFY(grl::flatbuffer::VerifyKUKAiiwaStatesBuffer(verifier)); - if(debug_) logger_->info("sending packet to KUKA iiwa: len = {}", fbbP->GetSize()); - int ret; - // Send UDP packet to Robot - ret = sendto(socket_local, fbbP->GetBufferPointer(), fbbP->GetSize(), 0, (struct sockaddr *)&dst_sockaddr, sizeof(dst_sockaddr)); - if (static_cast(ret) != static_cast(fbbP->GetSize())) logger_->error("Error sending packet to KUKA iiwa: ret = {}, len = {}", ret, fbbP->GetSize()); - - setArmConfiguration_ = false; - - // Receiving data from Sunrise - - int num; - temp_mask = mask; - - // tv is the time that you wait for a new message to arrive, - // since we don't wont to hinder the execution of each cycle, it is set to zero... - // (The FRI interface read rate is much higher than the data coming for the controller, so we don't wait for controller data) - struct timeval tv; - tv.tv_sec = 0; - tv.tv_usec = 0; - - // Check to see if any packets are available with waiting time of 0 - // Please note that with this configuration some packets may be dropped. - /// @todo TODO(ahundt) eventually run this in a separate thread so we can receive packets asap and minimize dropping - num = select(FD_SETSIZE, &temp_mask, &dummy_mask, &dummy_mask, &tv); - - if (num > 0) + if(armControlMode_ == flatbuffer::ArmState::MoveArmJointServo) + { + auto states2 = flatbuffer::GetKUKAiiwaStates(fbbP->GetBufferPointer()); + auto movearm = static_cast(states2->states()->Get(0)->armControlState()->state()); + std::vector angles; + for(std::size_t i = 0; i < movearm->goal()->position()->size(); ++i) { - // packets are available, process them - if (FD_ISSET(socket_local, &temp_mask)) - { - static const std::size_t udp_size = 1400; - unsigned char recbuf[udp_size]; - static const int flags = 0; - - ret = recvfrom(socket_local, recbuf, sizeof(recbuf), flags, (struct sockaddr *)&dst_sockaddr, &dst_sockaddr_len); - if (ret <= 0) logger_->error("C++ KukaJAVAdriver Error: Receive failed with ret = {}", ret); - - if (ret > 0){ - - if(debug_) logger_->info("C++ KukaJAVAdriver received message size: {}",ret); - - - auto rbPstart = static_cast(recbuf); - - auto verifier = flatbuffers::Verifier(rbPstart, ret); - auto bufOK = grl::flatbuffer::VerifyKUKAiiwaStatesBuffer(verifier); - - // Flatbuffer has been verified as valid - if (bufOK) { - // only reading the wrench data currently - auto bufff = static_cast(rbPstart); - if(debug_) logger_->info("C++ KukaJAVAdriver: flatbuffer verified successfully"); - - auto fbKUKAiiwaStates = grl::flatbuffer::GetKUKAiiwaStates(bufff); - auto wrench = fbKUKAiiwaStates->states()->Get(0)->monitorState()->CartesianWrench(); - - armState_.wrenchJava.clear(); - armState_.wrenchJava.push_back(wrench->force().x()); - armState_.wrenchJava.push_back(wrench->force().y()); - armState_.wrenchJava.push_back(wrench->force().z()); - armState_.wrenchJava.push_back(wrench->torque().x()); - armState_.wrenchJava.push_back(wrench->torque().y()); - armState_.wrenchJava.push_back(wrench->torque().z()); - - - } else { - logger_->error("C++ KukaJAVAdriver Error: flatbuff failed verification. bufOk: {}", bufOK); - } - - - } - - } + angles.push_back(movearm->goal()->position()->Get(i)); } + // logger_->info("re-extracted {}{}{}", movearm->goal()->position()->size(), " joint angles: ",angles); + } + if(debug_) logger_->info("sending packet to KUKA iiwa: len = {}", fbbP->GetSize()); + int ret; + // Send UDP packet to Robot + ret = sendto(socket_local, fbbP->GetBufferPointer(), fbbP->GetSize(), 0, (struct sockaddr *)&dst_sockaddr, sizeof(dst_sockaddr)); + + if (static_cast(ret) != static_cast(fbbP->GetSize())) logger_->error("Error sending packet to KUKA iiwa: ret = {}, len = {}", ret, fbbP->GetSize()); + + setArmConfiguration_ = false; + + // Receiving data from Sunrise + int num; + temp_mask = mask; + // tv is the time that you wait for a new message to arrive, + // since we don't wont to hinder the execution of each cycle, it is set to zero... + // (The FRI interface read rate is much higher than the data coming for the controller, so we don't wait for controller data) + struct timeval tv; + tv.tv_sec = 0; + tv.tv_usec = 0; + // Check to see if any packets are available with waiting time of 0 + // Please note that with this configuration some packets may be dropped. + /// @todo TODO(ahundt) eventually run this in a separate thread so we can receive packets asap and minimize dropping + num = select(FD_SETSIZE, &temp_mask, &dummy_mask, &dummy_mask, &tv); + + if (num > 0) + { + // packets are available, process them + if (FD_ISSET(socket_local, &temp_mask)) + { + // allocate the buffer, should only happen once + if(!java_interface_received_statesP_) { + java_interface_received_statesP_ = std::make_shared>(fbs_tk::Buffer(udp_size_)); + } + if(!java_interface_next_statesP_) { + java_interface_next_statesP_ = std::make_shared>(fbs_tk::Buffer(udp_size_)); + } + static const int flags = 0; + // get a reference to the buffer object + const fbs_tk::Buffer& internal_buffer = java_interface_received_statesP_->get_data(); + // receive an update from the java driver over UDP + std::vector bufferdata = internal_buffer.get_data(); + ret = recvfrom(socket_local, reinterpret_cast(&bufferdata[0]), internal_buffer.size(), flags, (struct sockaddr *)&dst_sockaddr, &dst_sockaddr_len); + if (ret <= 0) { + bool java_state_received_successfully = false; + logger_->error("C++ KukaJAVAdriver Error: Receive failed with ret = {}", ret); + } else { + if(debug_) logger_->info("C++ KukaJAVAdriver received message size: {}",ret); + java_interface_received_statesP_->update_root(); + // Flatbuffer has been verified as valid + if (java_interface_received_statesP_->valid()) { + bool java_state_received_successfully = true; + std::swap(java_interface_received_statesP_, java_interface_next_statesP_); + if(debug_) logger_->info("C++ KukaJAVAdriver: flatbuffer verified successfully"); + } else { + // TODO(ahundt) consider specific error codes for verifier failure vs udp receive failure + bool java_state_received_successfully = false; + // logger_->error("C++ KukaJAVAdriver Error: flatbuff failed verification. bufOk: {}", java_state_received_successfully); + } + } + } + } + return haveNewData; + } - return haveNewData; - } - - volatile std::size_t m_haveReceivedRealDataCount = 0; - volatile std::size_t m_attemptedCommunicationCount = 0; - volatile std::size_t m_attemptedCommunicationConsecutiveFailureCount = 0; - volatile std::size_t m_attemptedCommunicationConsecutiveSuccessCount = 0; - - - - void setPositionControlMode() - { - boost::lock_guard lock(jt_mutex); - controlMode_ = grl::flatbuffer::EControlMode::POSITION_CONTROL_MODE; - setArmConfiguration_ = true; - } - - - bool setJointImpedanceMode(std::vector joint_stiffnes, std::vectorjoint_damping) { - boost::lock_guard lock(jt_mutex); - //TODO use tags - joint_stiffness_ = joint_stiffnes; - joint_damping_ = joint_damping; - controlMode_ = grl::flatbuffer::EControlMode::JOINT_IMP_CONTROL_MODE; - setArmConfiguration_ = true; - } - - // TODO: define custom flatbuffer for Cartesion Quantities - void setCartesianImpedanceMode( - const grl::flatbuffer::EulerPose cart_stiffness, const grl::flatbuffer::EulerPose cart_damping, - const double nullspace_stiffness, const double nullspace_damping, - const grl::flatbuffer::EulerPose cart_max_path_deviation, - const grl::flatbuffer::EulerPose cart_max_ctrl_vel, - const grl::flatbuffer::EulerPose cart_max_ctrl_force, - const bool max_control_force_stop) - { - boost::lock_guard lock(jt_mutex); + volatile std::size_t m_haveReceivedRealDataCount = 0; + volatile std::size_t m_attemptedCommunicationCount = 0; + volatile std::size_t m_attemptedCommunicationConsecutiveFailureCount = 0; + volatile std::size_t m_attemptedCommunicationConsecutiveSuccessCount = 0; + void setPositionControlMode() + { + boost::lock_guard lock(jt_mutex); + controlMode_ = grl::flatbuffer::EControlMode::POSITION_CONTROL_MODE; + setArmConfiguration_ = true; + } + bool setJointImpedanceMode(std::vector joint_stiffnes, std::vectorjoint_damping) { + boost::lock_guard lock(jt_mutex); + //TODO use tags + joint_stiffness_ = joint_stiffnes; + joint_damping_ = joint_damping; + controlMode_ = grl::flatbuffer::EControlMode::JOINT_IMP_CONTROL_MODE; + setArmConfiguration_ = true; + } + // TODO: define custom flatbuffer for Cartesion Quantities + void setCartesianImpedanceMode( + const grl::flatbuffer::EulerPose cart_stiffness, + const grl::flatbuffer::EulerPose cart_damping, + const double nullspace_stiffness, + const double nullspace_damping, + const grl::flatbuffer::EulerPose cart_max_path_deviation, + const grl::flatbuffer::EulerPose cart_max_ctrl_vel, + const grl::flatbuffer::EulerPose cart_max_ctrl_force, + const bool max_control_force_stop) + { + boost::lock_guard lock(jt_mutex); - cart_stiffness_ = cart_stiffness; - cart_damping_ = cart_damping; + cart_stiffness_ = cart_stiffness; + cart_damping_ = cart_damping; - cart_max_path_deviation_ = cart_max_path_deviation; - cart_max_ctrl_vel_ = cart_max_ctrl_vel; - cart_max_ctrl_force_ = cart_max_ctrl_force; + cart_max_path_deviation_ = cart_max_path_deviation; + cart_max_ctrl_vel_ = cart_max_ctrl_vel; + cart_max_ctrl_force_ = cart_max_ctrl_force; - nullspace_stiffness_ = nullspace_stiffness; - nullspace_damping_ = nullspace_damping; + nullspace_stiffness_ = nullspace_stiffness; + nullspace_damping_ = nullspace_damping; - max_control_force_stop_ = max_control_force_stop; + max_control_force_stop_ = max_control_force_stop; - controlMode_ = grl::flatbuffer::EControlMode::CART_IMP_CONTROL_MODE; - setArmConfiguration_ = true; - } + controlMode_ = grl::flatbuffer::EControlMode::CART_IMP_CONTROL_MODE; + setArmConfiguration_ = true; + } /** * \brief Set the joint positions for the current interpolation step. @@ -487,10 +466,10 @@ namespace grl { namespace robot { namespace arm { */ template void set(Range&& range, grl::revolute_joint_angle_open_chain_command_tag) { - boost::lock_guard lock(jt_mutex); - armState_.clearCommands(); - boost::copy(range, std::back_inserter(armState_.commandedPosition)); - boost::copy(range, std::back_inserter(armState_.commandedPosition_goal)); + boost::lock_guard lock(jt_mutex); + armState_.clearCommands(); + boost::copy(range, std::back_inserter(armState_.commandedPosition)); + boost::copy(range, std::back_inserter(armState_.commandedPosition_goal)); } /** @@ -505,8 +484,8 @@ namespace grl { namespace robot { namespace arm { * @brief set the interface over which state is monitored (FRI interface, alternately SmartServo/DirectServo == JAVA interface, ) */ void set(flatbuffer::KUKAiiwaInterface mif, state_tag) { - boost::lock_guard lock(jt_mutex); - commandInterface_ = mif; + boost::lock_guard lock(jt_mutex); + commandInterface_ = mif; } /** @@ -525,8 +504,8 @@ namespace grl { namespace robot { namespace arm { */ template void set(TimeDuration && duration_to_goal_command, time_duration_command_tag) { - boost::lock_guard lock(jt_mutex); - armState_.goal_position_command_time_duration = duration_to_goal_command; + boost::lock_guard lock(jt_mutex); + armState_.goal_position_command_time_duration = duration_to_goal_command; } @@ -539,13 +518,18 @@ namespace grl { namespace robot { namespace arm { * @see KukaFRIdriver::set(Range&& range, grl::revolute_joint_angle_open_chain_command_tag) * */ - KukaState::time_point_type get(time_point_tag) { + // KukaState::time_point_type get(time_point_tag) { + // boost::lock_guard lock(jt_mutex); + // return armState_.timestamp; + // } + cartographer::common::Time get(time_point_tag) { boost::lock_guard lock(jt_mutex); - return armState_.timestamp; + return armState_.time_event_stamp.device_time; } + /** * \brief Set the applied joint torques for the current interpolation step. * @@ -557,40 +541,40 @@ namespace grl { namespace robot { namespace arm { * @param torques Array with the applied torque values (in Nm) * @param tag identifier object indicating that the torqe value command should be modified */ - template - void set(Range&& range, grl::revolute_joint_torque_open_chain_command_tag) { - boost::lock_guard lock(jt_mutex); - armState_.clearCommands(); - boost::copy(range, armState_.commandedTorque); + template + void set(Range&& range, grl::revolute_joint_torque_open_chain_command_tag) { + boost::lock_guard lock(jt_mutex); + armState_.clearCommands(); + boost::copy(range, armState_.commandedTorque); } - /** - * \brief Set the applied wrench vector of the current interpolation step. - * - * The wrench vector consists of: - * [F_x, F_y, F_z, tau_A, tau_B, tau_C] - * - * F ... forces (in N) applied along the Cartesian axes of the - * currently used motion center. - * tau ... torques (in Nm) applied along the orientation angles - * (Euler angles A, B, C) of the currently used motion center. - * - * This method is only effective when the client is in a commanding state. - * The ControlMode of the robot has to be Cartesian impedance control mode. The - * Client Command Mode has to be wrench. - * - * @param state object storing the command data that will be sent to the physical device - * @param range wrench Applied Cartesian wrench vector, in x, y, z, roll, pitch, yaw force measurments. - * @param tag identifier object indicating that the wrench value command should be modified - * - * @todo perhaps support some specific more useful data layouts - */ + /** + * \brief Set the applied wrench vector of the current interpolation step. + * + * The wrench vector consists of: + * [F_x, F_y, F_z, tau_A, tau_B, tau_C] + * + * F ... forces (in N) applied along the Cartesian axes of the + * currently used motion center. + * tau ... torques (in Nm) applied along the orientation angles + * (Euler angles A, B, C) of the currently used motion center. + * + * This method is only effective when the client is in a commanding state. + * The ControlMode of the robot has to be Cartesian impedance control mode. The + * Client Command Mode has to be wrench. + * + * @param state object storing the command data that will be sent to the physical device + * @param range wrench Applied Cartesian wrench vector, in x, y, z, roll, pitch, yaw force measurments. + * @param tag identifier object indicating that the wrench value command should be modified + * + * @todo perhaps support some specific more useful data layouts + */ template void set(Range&& range, grl::cartesian_wrench_command_tag) { - boost::lock_guard lock(jt_mutex); - armState_.clearCommands(); - std::copy(range,armState_.commandedCartesianWrenchFeedForward); + boost::lock_guard lock(jt_mutex); + armState_.clearCommands(); + std::copy(range,armState_.commandedCartesianWrenchFeedForward); } /// @todo should this exist? is it written correctly? @@ -600,13 +584,51 @@ namespace grl { namespace robot { namespace arm { state = armState_; } - void getWrench(KukaState & state) - { - boost::lock_guard lock(jt_mutex); + /// get 6 element wrench entries + /// [force_x, force_y, force_z, torque_x, torque_y, torque_z] + /* + template + void getWrench(OutputIterator output) + { + boost::lock_guard lock(jt_mutex); + + // make sure the object exists and contains data + if (java_interface_received_statesP_ && java_interface_received_statesP_->valid()) + { + std::cout<< "eqTypes Test: " << eqTypes() << std::endl; + // T* operator->() overload + // https://stackoverflow.com/questions/38542766/why-the-t-operator-is-applied-repeatedly-even-if-written-once + auto wrench = (*java_interface_received_statesP_)->states()->Get(0)->monitorState()->CartesianWrench(); + // insert the values into the output vector + *output++ = wrench->force().x(); // double + *output++ = wrench->force().y(); + *output++ = wrench->force().z(); + *output++ = wrench->torque().x(); + *output++ = wrench->torque().y(); + *output++ = wrench->torque().z(); + } + } + */ + template + void getWrench(Container output) + { + boost::lock_guard lock(jt_mutex); - if (!armState_.wrenchJava.empty()) { - state.wrenchJava = armState_.wrenchJava; - } + // make sure the object exists and contains data + if (java_interface_received_statesP_ && java_interface_received_statesP_->valid()) + { + std::cout<< "eqTypes Test: " << eqTypes() << std::endl; + // T* operator->() overload + // https://stackoverflow.com/questions/38542766/why-the-t-operator-is-applied-repeatedly-even-if-written-once + auto wrench = (*java_interface_received_statesP_)->states()->Get(0)->monitorState()->CartesianWrench(); + // insert the values into the output vector + output.push_back(wrench->force().x()); + output.push_back(wrench->force().y()); + output.push_back(wrench->force().z()); + output.push_back(wrench->torque().x()); + output.push_back(wrench->torque().y()); + output.push_back(wrench->torque().z()); + } } /// set the mode of the arm. Examples: Teach or MoveArmJointServo @@ -625,8 +647,23 @@ namespace grl { namespace robot { namespace arm { armControlMode = armControlMode_; } + ///////////////////////////////////////////////////////////////////// + /* Helper function for checking type equality in compile-time. */ + /// https://stackoverflow.com/questions/16924168/compile-time-function-for-checking-type-equality + template + struct is_same : std::false_type { }; + + template + struct is_same : std::true_type { }; + + template + constexpr bool eqTypes() { return is_same::value; } + ///////////////////////////////////////////////////////////////////// + private: + /// @TODO(ahundt) don't assume this fixed size, consider a parameter + static const std::size_t udp_size_ = 1400; std::shared_ptr logger_; int socket_local; int port; @@ -634,10 +671,21 @@ namespace grl { namespace robot { namespace arm { socklen_t dst_sockaddr_len = sizeof(dst_sockaddr); struct sockaddr_in local_sockaddr; long dst_ip; + /// An fd_set is a set of sockets to "monitor" for some activity (set of socket descriptors). fd_set mask, temp_mask, dummy_mask; Params params_; - KukaState armState_; + KukaState armState_; // structure defined in Kuka.hpp + // this is the data that was received from the remote computer + // with the data from the java interface that gets sent back and forth + // this one will only contain valid received data + std::shared_ptr> java_interface_received_statesP_; + /// indicates if the next state was received successfully in java_interface_next_statesP_ + /// and java_interface_received_statesP_ was updated accordingly. + bool java_state_received_successfully = false; + /// used for receiving the next buffer over the network. + /// if some receive calls fail this one may not contain valid data. + std::shared_ptr> java_interface_next_statesP_; // armControlMode is the current GRL_Driver.java configuration to which the arm is currently set. // Options are: // ArmState_NONE = 0, @@ -651,7 +699,7 @@ namespace grl { namespace robot { namespace arm { // ArmState_MoveArmCartesianServo = 8 flatbuffer::ArmState armControlMode_; flatbuffer::KUKAiiwaInterface commandInterface_ = flatbuffer::KUKAiiwaInterface::SmartServo;// KUKAiiwaInterface::SmartServo; - flatbuffer::KUKAiiwaInterface monitorInterface_ = flatbuffer::KUKAiiwaInterface::FRI; + flatbuffer::KUKAiiwaInterface monitorInterface_ = flatbuffer::KUKAiiwaInterface::FRI; // flatbuffers::FlatBufferBuilder builder_; // // flatbuffer::JointStateBuilder jointStateServoBuilder_; @@ -697,7 +745,7 @@ namespace grl { namespace robot { namespace arm { std::vector joint_stiffness_; std::vector joint_damping_; - }; + }; // End of class }}}// namespace grl::robot::arm diff --git a/include/grl/kuka/KukaToFlatbuffer.hpp b/include/grl/kuka/KukaToFlatbuffer.hpp new file mode 100644 index 0000000..d75615f --- /dev/null +++ b/include/grl/kuka/KukaToFlatbuffer.hpp @@ -0,0 +1,759 @@ +#ifndef GRL_KUKA_TO_FLATBUFFER +#define GRL_KUKA_TO_FLATBUFFER + +/// Before including any FlatBuffers related headers, you can add this #define. +/// You'll get an assert whenever the verifier fails, whose stack-trace can tell you exactly what check failed an on what field etc. +#define FLATBUFFERS_DEBUG_VERIFICATION_FAILURE + +#include + +#include "grl/flatbuffer/JointState_generated.h" +#include "grl/flatbuffer/ArmControlState_generated.h" +#include "grl/flatbuffer/KUKAiiwa_generated.h" +#include "grl/flatbuffer/LinkObject_generated.h" +#include "grl/flatbuffer/Euler_generated.h" +#include "grl/flatbuffer/HelperToFlatbuffer.hpp" +// #include "KukaFRIdriver.hpp" +#include "Kuka.hpp" +#include "KukaFRIalgorithm.hpp" +#include + +#include +#include +namespace grl { + +/// faltbuffer enum objects +/// 1. Which element in the enum should be selected as the defaut return value? +/// 2. Is the argument like enum, int, double etc. passed by value or by reference? + +grl::flatbuffer::ESessionState toFlatBuffer(const ::FRISessionState sessionState) { + + switch(sessionState) { + case FRISessionState_MONITORING_WAIT: + return grl::flatbuffer::ESessionState::MONITORING_WAIT; + case FRISessionState_MONITORING_READY: + return grl::flatbuffer::ESessionState::MONITORING_READY; + case FRISessionState_COMMANDING_WAIT: + return grl::flatbuffer::ESessionState::COMMANDING_WAIT; + case FRISessionState_COMMANDING_ACTIVE: + return grl::flatbuffer::ESessionState::COMMANDING_ACTIVE; + default: + break; + } + return grl::flatbuffer::ESessionState::IDLE; +} + +grl::flatbuffer::EConnectionQuality toFlatBuffer(const ::FRIConnectionQuality connectionQuality) { + + switch(connectionQuality) { + case FRIConnectionQuality_POOR: + return grl::flatbuffer::EConnectionQuality::POOR; + case FRIConnectionQuality_FAIR: + return grl::flatbuffer::EConnectionQuality::FAIR; + case FRIConnectionQuality_GOOD: + return grl::flatbuffer::EConnectionQuality::GOOD; + + default: + break; + } + return grl::flatbuffer::EConnectionQuality::EXCELLENT; +} + +grl::flatbuffer::ESafetyState toFlatBuffer(const ::SafetyState safetyState) { + + switch(safetyState) { + case SafetyState_SAFETY_STOP_LEVEL_0: + return grl::flatbuffer::ESafetyState::SAFETY_STOP_LEVEL_0; + case SafetyState_SAFETY_STOP_LEVEL_1: + return grl::flatbuffer::ESafetyState::SAFETY_STOP_LEVEL_1; + case SafetyState_SAFETY_STOP_LEVEL_2: + return grl::flatbuffer::ESafetyState::SAFETY_STOP_LEVEL_2; + + default: + break; + } + return grl::flatbuffer::ESafetyState::NORMAL_OPERATION; +} + +grl::flatbuffer::EOperationMode toFlatBuffer(const ::OperationMode operationMode) { + + switch(operationMode) { + case OperationMode_TEST_MODE_1: + return grl::flatbuffer::EOperationMode::TEST_MODE_1; + case OperationMode_TEST_MODE_2: + return grl::flatbuffer::EOperationMode::TEST_MODE_2; + + default: + break; + } + return grl::flatbuffer::EOperationMode::AUTOMATIC_MODE; +} + +grl::flatbuffer::EDriveState toFlatBuffer(const ::DriveState driveState) { + + switch(driveState) { + case DriveState_OFF: + return grl::flatbuffer::EDriveState::OFF; + case DriveState_TRANSITIONING: + return grl::flatbuffer::EDriveState::TRANSITIONING; + + default: // DriveState_ACTIVE + break; + } + return grl::flatbuffer::EDriveState::ACTIVE; +} + +grl::flatbuffer::EControlMode toFlatBuffer(const ::ControlMode controlMode) { + + switch(controlMode) { + case ControlMode_POSITION_CONTROLMODE: + return grl::flatbuffer::EControlMode::POSITION_CONTROL_MODE; + case ControlMode_CARTESIAN_IMPEDANCE_CONTROLMODE: + return grl::flatbuffer::EControlMode::CART_IMP_CONTROL_MODE; + case ControlMode_JOINT_IMPEDANCE_CONTROLMODE: + return grl::flatbuffer::EControlMode::JOINT_IMP_CONTROL_MODE; + default: // ControlMode_NO_CONTROLMODE + break; + } + return grl::flatbuffer::EControlMode::NO_CONTROL; +} + +grl::flatbuffer::EClientCommandMode toFlatBuffer(const ::ClientCommandMode clientcommandMode) { + + switch(clientcommandMode) { + case ClientCommandMode_POSITION: + return grl::flatbuffer::EClientCommandMode::POSITION; + case ClientCommandMode_WRENCH: + return grl::flatbuffer::EClientCommandMode::WRENCH; + case ClientCommandMode_TORQUE: + return grl::flatbuffer::EClientCommandMode::TORQUE; + + default: // ClientCommandMode_NO_COMMAND_MODE + break; + } + return grl::flatbuffer::EClientCommandMode::NO_COMMAND_MODE; +} + +grl::flatbuffer::EOverlayType toFlatBuffer(const ::OverlayType overlayType) { + + switch(overlayType) { + + case OverlayType_JOINT: + return grl::flatbuffer::EOverlayType::JOINT; + case OverlayType_CARTESIAN: + return grl::flatbuffer::EOverlayType::CARTESIAN; + + default: // OverlayType_NO_OVERLAY + break; + } + return grl::flatbuffer::EOverlayType::NO_OVERLAY; +} +/// Euler.fbs +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const double x, + const double y, + const double z) +{ + return grl::flatbuffer::CreateEulerTranslationParams(fbb, x, y, z); +} +/// Euler.fbs +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const double r1, + const double r2, + const double r3, + grl::flatbuffer::EulerOrder &eulerOrder) +{ + return grl::flatbuffer::CreateEulerRotationParams(fbb, r1, r2, r3, eulerOrder); +} + +/// Euler.fbs, struct EulerRotation +grl::flatbuffer::EulerRotation toFlatBuffer( + const Eigen::Vector3d &pt, + grl::flatbuffer::EulerOrder eulerOrder ) +{ + return grl::flatbuffer::EulerRotation(pt.x(), pt.y(), pt.z(), eulerOrder); +} +/// Euler.fbs, struct EulerPose +grl::flatbuffer::EulerPose toFlatBuffer( + const grl::flatbuffer::Vector3d &positon, + const grl::flatbuffer::EulerRotation &eulerRotation) +{ + return grl::flatbuffer::EulerPose(positon, eulerRotation); +} +/// Euler.fbs +/// Overload the above function +/// TODO (@Chunting) Check if ptr has the same physical meaning with pt, if so, discard it. +grl::flatbuffer::EulerPose toFlatBuffer( + const Eigen::Vector3d &pt, + const Eigen::Vector3d &ptr, + grl::flatbuffer::EulerOrder eulerOrder) +{ + auto positon = grl::toFlatBuffer(pt); + auto eulerRotation = toFlatBuffer(ptr,eulerOrder); + return grl::flatbuffer::EulerPose(positon, eulerRotation); +} + +/// Euler.fbs +/// tables EulerPose and EulerPoseParams are both defined in Euler.fbs. +/// Like EulerPose, this function can be also overloaded with Eigen arguments. +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const grl::flatbuffer::Vector3d &position, + const grl::flatbuffer::EulerRotation &rotation) +{ + return grl::flatbuffer::CreateEulerPoseParams(fbb, &position, &rotation); +} + +/// LinkObject.fbs +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const std::string &name, + const std::string &parent, + const grl::flatbuffer::Pose &pose, // Using the helper function toFlatBuffer to get this parameter? + const grl::flatbuffer::Inertia &inertia) +{ + return grl::flatbuffer::CreateLinkObject( + fbb, + fbb.CreateString(name), + fbb.CreateString(parent), + std::addressof(pose), + std::addressof(inertia)); +} + +/// JointState.fbs +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const std::vector &position, + const std::vector &velocity, + const std::vector &acceleration, + const std::vector &torque) +{ + return grl::flatbuffer::CreateJointState( + fbb, + !position.empty() ? fbb.CreateVector(position):0, + !velocity.empty() ? fbb.CreateVector(velocity):0, + !acceleration.empty() ? fbb.CreateVector(acceleration):0, + !torque.empty() ? fbb.CreateVector(torque):0); +} +/// JointState.fbs +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const std::vector& kukaStates, + const std::vector &velocity, + const std::vector &acceleration) +{ + std::vector position; + std::vector torque; + std::size_t sizeofStates = kukaStates.size(); + for(auto &kukaState : kukaStates){ + + boost::copy(kukaState.position, &position[0]); + boost::copy(kukaState.torque, &torque[0]); + } + return grl::flatbuffer::CreateJointState( + fbb, + fbb.CreateVector(position), + fbb.CreateVector(velocity), + fbb.CreateVector(acceleration), + fbb.CreateVector(torque)); +} + + +/// ArmControlState.fbs +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const std::vector> &traj) +{ + return grl::flatbuffer::CreateMoveArmTrajectory( + fbb, + fbb.CreateVector>(traj)); +} + +/// ArmControlState.fbs +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + flatbuffers::Offset &goal) +{ + return grl::flatbuffer::CreateMoveArmJointServo (fbb, goal); +} +// /// ArmControlState.fbs +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const std::string &parent, + const grl::flatbuffer::Pose &goal) +{ + return grl::flatbuffer::CreateMoveArmCartesianServo( + fbb, + fbb.CreateString(parent), + std::addressof(goal)); +} + + +/// ArmControlState.fbs +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const std::string &name, + int64_t sequenceNumber, + double timeStamp, + grl::flatbuffer::ArmState &armControlMode) +{ + // The parameter of ArmState is undetermined. + // grl::flatbuffer::ArmState armstate_type = grl::flatbuffer::ArmState::armstate; + // auto command = grl::flatbuffer::CreateStartArm(fbb); + switch (armControlMode) { + case grl::flatbuffer::ArmState::StartArm: { + return grl::flatbuffer::CreateArmControlState( + fbb, + fbb.CreateString(name), + sequenceNumber, + timeStamp, + armControlMode, + grl::flatbuffer::CreateStartArm(fbb).Union()); + } + case grl::flatbuffer::ArmState::TeachArm: { + return grl::flatbuffer::CreateArmControlState( + fbb, + fbb.CreateString(name), + sequenceNumber, + timeStamp, + armControlMode, + grl::flatbuffer::CreateTeachArm(fbb).Union()); + } + case grl::flatbuffer::ArmState::PauseArm: { + return grl::flatbuffer::CreateArmControlState( + fbb, + fbb.CreateString(name), + sequenceNumber, + timeStamp, + armControlMode, + grl::flatbuffer::CreatePauseArm(fbb).Union()); + } + case grl::flatbuffer::ArmState::StopArm: { + return grl::flatbuffer::CreateArmControlState( + fbb, + fbb.CreateString(name), + sequenceNumber, + timeStamp, + armControlMode, + grl::flatbuffer::CreateStopArm(fbb).Union()); + } + case grl::flatbuffer::ArmState::ShutdownArm: { + return grl::flatbuffer::CreateArmControlState( + fbb, + fbb.CreateString(name), + sequenceNumber, + timeStamp, + armControlMode, + grl::flatbuffer::CreateShutdownArm(fbb).Union()); + } + case grl::flatbuffer::ArmState::NONE: { + //std::cout << "Waiting for interation mode... (currently NONE)" << std::endl; + break; + } + default: + std::cout<< "C++ KukaJAVAdriver: unsupported use case: " << EnumNameArmState(armControlMode) << std::endl; + } +} + +/// ArmControlState.fbs +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const std::string &name, + int64_t sequenceNumber, + double timeStamp, + grl::robot::arm::KukaState &armState, + grl::flatbuffer::ArmState &armControlMode) +{ + switch (armControlMode) { + // std::cout<< "C++ KukaJAVAdriver: sending armposition command: " << armState.commandedPosition_goal << std::endl; + case grl::flatbuffer::ArmState::MoveArmJointServo: { + auto armPositionBuffer = fbb.CreateVector(armState.commandedPosition_goal.data(), armState.commandedPosition_goal.size()); + auto commandedTorque = fbb.CreateVector(armState.commandedTorque.data(), armState.commandedTorque.size()); + auto goalJointState = grl::flatbuffer::CreateJointState(fbb,armPositionBuffer,0/*no velocity*/,0/*no acceleration*/,commandedTorque); + auto moveArmJointServo = grl::flatbuffer::CreateMoveArmJointServo(fbb,goalJointState); + return grl::flatbuffer::CreateArmControlState( + fbb, + fbb.CreateString(name), + sequenceNumber, + timeStamp, + armControlMode, + moveArmJointServo.Union()); + } + default: + toFlatBuffer(fbb, name, sequenceNumber, timeStamp, armControlMode); + } +} + +/// ArmControlState.fbs +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const std::vector> &armcontrolstates) +{ + return grl::flatbuffer::CreateArmControlSeries( + fbb, + fbb.CreateVector>(armcontrolstates)); +} +/// KUKAiiwa.fbs +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const grl::flatbuffer::EulerPose& stiffness, + const grl::flatbuffer::EulerPose& damping, + const double nullspaceStiffness, + const double nullspaceDamping, + const grl::flatbuffer::EulerPose &maxPathDeviation, + const grl::flatbuffer::EulerPose &maxCartesianVelocity, + const grl::flatbuffer::EulerPose &maxControlForce, + const bool maxControlForceExceededStop) +{ + return grl::flatbuffer::CreateCartesianImpedenceControlMode( + fbb, + std::addressof(stiffness), + std::addressof(damping), + nullspaceStiffness, + nullspaceDamping, + std::addressof(maxPathDeviation), + std::addressof(maxCartesianVelocity), + std::addressof(maxControlForce), + maxControlForceExceededStop); +} +/// KUKAiiwa.fbs +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + std::vector &joint_stiffness, + std::vector &joint_damping) +{ + auto jointStiffnessBuffer = fbb.CreateVector(joint_stiffness.data(),joint_stiffness.size()); + auto jointDampingBuffer = fbb.CreateVector(joint_damping.data(),joint_damping.size()); + return grl::flatbuffer::CreateJointImpedenceControlMode(fbb, jointStiffnessBuffer, jointDampingBuffer); +} +/// KUKAiiwa.fbs +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const ::OverlayType &overlayType, + const ::ConnectionInfo &connectionInfo, + const bool updatePortOnRemote, + const int16_t portOnRemote, + const bool updatePortOnController, + const int16_t portOnController) +{ + return grl::flatbuffer::CreateFRI( + fbb, + toFlatBuffer(overlayType), + connectionInfo.sendPeriod, + connectionInfo.receiveMultiplier, + updatePortOnRemote, + portOnRemote, + updatePortOnController, + portOnController + ); +} + +/// KUKAiiwa.fbs +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const std::vector &jointAccelerationRel, + const std::vector &jointVelocityRel, + const bool updateMinimumTrajectoryExecutionTime, + const double minimumTrajectoryExecutionTime) +{ + auto jointAccelerationRelBuffer = fbb.CreateVector(jointAccelerationRel.data(),jointAccelerationRel.size()); + auto jointVelocityRelBuffer = fbb.CreateVector(jointVelocityRel.data(),jointVelocityRel.size()); + return grl::flatbuffer::CreateSmartServo( + fbb, + jointAccelerationRelBuffer, + jointAccelerationRelBuffer, + updateMinimumTrajectoryExecutionTime, + minimumTrajectoryExecutionTime + ); +} + +/// KUKAiiwa.fbs +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const std::string &dataType, + const std::string &defaultValue, + const std::string &displayName, + const std::string &id, + const std::string &min, + const std::string &max, + const std::string &unit, + const std::string &value, + bool shouldRemove = false, + bool shouldUpdate = false) +{ + return grl::flatbuffer::CreateProcessData( + fbb, + fbb.CreateString(dataType), + fbb.CreateString(defaultValue), + fbb.CreateString(displayName), + fbb.CreateString(id), + fbb.CreateString(min), + fbb.CreateString(max), + fbb.CreateString(unit), + fbb.CreateString(value), + shouldRemove, + shouldUpdate); +} + +/// KUKAiiwa.fbs +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const std::string &name, + const grl::flatbuffer::KUKAiiwaInterface &commandInterface, // enum defined in KUKAiiwa.fbs + const grl::flatbuffer::KUKAiiwaInterface &monitorInterface, + const ::ClientCommandMode &clientCommandMode, // enum + const ::OverlayType &overlayType, // enum + const ::ControlMode &controlMode, // enum + flatbuffers::Offset &setCartImpedance, + flatbuffers::Offset &setJointImpedance, + flatbuffers::Offset &smartServoConfig, + flatbuffers::Offset &FRIConfig, + const std::vector> &tools, + const std::vector> &processData, + const std::string ¤tMotionCenter, + bool requestMonitorProcessData = false) +{ + return grl::flatbuffer::CreateKUKAiiwaArmConfiguration( + fbb, + fbb.CreateString(name), + commandInterface, + monitorInterface, + toFlatBuffer(clientCommandMode), + toFlatBuffer(overlayType), + toFlatBuffer(controlMode), + setCartImpedance, + setJointImpedance, + smartServoConfig, + FRIConfig, + fbb.CreateVector>(tools), + fbb.CreateVector>(processData), + fbb.CreateString(currentMotionCenter), + requestMonitorProcessData); +} + +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const std::string &hardwareVersion, + const std::vector &torqueSensorLimits, + bool isReadyToMove, + bool isMastered, + const std::vector> &processData) + { + return grl::flatbuffer::CreateKUKAiiwaMonitorConfiguration( + fbb, + fbb.CreateString(hardwareVersion), + fbb.CreateVector(torqueSensorLimits), + isReadyToMove, + isMastered, + fbb.CreateVector>(processData)); + } + +/// KUKAiiwa.fbs +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + flatbuffers::Offset &measuredState, + const grl::flatbuffer::Pose &cartesianFlangePose, + flatbuffers::Offset &jointStateReal, + flatbuffers::Offset &jointStateInterpolated, + flatbuffers::Offset &externalState, + const ::FRISessionState &sessionState, + const ::OperationMode &operationMode, + const grl::flatbuffer::Wrench &CartesianWrench) +{ + return grl::flatbuffer::CreateKUKAiiwaMonitorState( + fbb, + measuredState, + std::addressof(cartesianFlangePose), + jointStateReal, + jointStateInterpolated, + externalState, + toFlatBuffer(operationMode), + toFlatBuffer(sessionState), + std::addressof(CartesianWrench) + ); +} + +/// KUKAiiwa.fbs +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const std::string &name, + const std::string &destination, + const std::string &source, + // const double timestamp, + const grl::TimeEvent &timeEvent, + const bool setArmControlState, + flatbuffers::Offset &armControlState, + const bool setArmConfiguration, + flatbuffers::Offset &armConfiguration, + const bool hasMonitorState, // MessageMonitorData monitorData in FRIMessage.pb.h + flatbuffers::Offset &monitorState, + const bool hasMonitorConfig, + flatbuffers::Offset &monitorConfig) +{ + + flatbuffers::Offset _timeEvent = toFlatBuffer(fbb, timeEvent); + + return grl::flatbuffer::CreateKUKAiiwaState( + fbb, + fbb.CreateString(name), + fbb.CreateString(destination), + fbb.CreateString(source), + _timeEvent, + setArmControlState, + armControlState, + setArmConfiguration, + armConfiguration, + hasMonitorState, + monitorState, + hasMonitorConfig, + monitorConfig); +} + + + +// KUKAiiwa.fbs +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const ::FRISessionState &sessionState, + const ::FRIConnectionQuality &connectionQuality, + const ::ControlMode &controlMode, // enum + const ::FRIMonitoringMessage &friMonitoringMessage, + const grl::TimeEvent &timeEvent) // There are two times (TimeStamp and TimeEvent) here, which one should be kept? both? +{ + auto _sessionState = toFlatBuffer(sessionState); + auto _connectionQuality = toFlatBuffer(connectionQuality); + auto _controlMode = toFlatBuffer(controlMode); + auto _messageIdentifier = friMonitoringMessage.header.messageIdentifier; + auto _sequenceCounter = friMonitoringMessage.header.sequenceCounter; + auto _reflectedSequenceCounter = friMonitoringMessage.header.reflectedSequenceCounter; + std::vector data; + // get measured joint + grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_angle_open_chain_state_tag()); + flatbuffers::Offset> _measuredJointPosition = fbb.CreateVector(data); + + data.clear(); + // get measured joint torque + grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_torque_open_chain_state_tag()); + flatbuffers::Offset> _measuredTorque = fbb.CreateVector(data); + + data.clear(); + // get measured joint torque + grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_angle_open_chain_command_tag()); + flatbuffers::Offset> _commandedJointPosition = fbb.CreateVector(data); + data.clear(); + // get commanded joint torque + grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_torque_open_chain_command_tag()); + flatbuffers::Offset> _commandedTorque = fbb.CreateVector(data); + + data.clear(); + data.clear(); + // get measured external torque + grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_torque_external_open_chain_state_tag()); + flatbuffers::Offset> _externalTorque = fbb.CreateVector(data); + + data.clear(); + // get interpolated joint state + grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_angle_interpolated_open_chain_state_tag()); + flatbuffers::Offset> _jointStateInterpolated = fbb.CreateVector(data); + + flatbuffers::Offset _timeEvent = toFlatBuffer(fbb, timeEvent); + auto _overlayType = toFlatBuffer(friMonitoringMessage.ipoData.overlayType); + + return grl::flatbuffer::CreateFRIMessageLog( + fbb, + _sessionState, + _connectionQuality, + _controlMode, + _messageIdentifier, + _sequenceCounter, + _reflectedSequenceCounter, + // _sec, + // _nanosec, + _measuredJointPosition, + _measuredTorque, + _commandedJointPosition, + _commandedTorque, + _externalTorque, + _jointStateInterpolated, + _timeEvent); +} +/// KUKAiiwa.fbs +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const std::string &name, + const std::string &destination, + const std::string &source, + // const double timestamp, + const grl::TimeEvent &timeEvent, + const bool setArmControlState, + const flatbuffers::Offset &armControlState, + const bool setArmConfiguration, + const flatbuffers::Offset &armConfiguration, + const bool hasMonitorState, + const flatbuffers::Offset &monitorState, + const bool hasMonitorConfig, + const flatbuffers::Offset &monitorConfig, + const flatbuffers::Offset &FRIMessage) + +{ + flatbuffers::Offset _timeEvent = grl::toFlatBuffer(fbb, timeEvent); + return grl::flatbuffer::CreateKUKAiiwaState( + fbb, + fbb.CreateString(name), + fbb.CreateString(destination), + fbb.CreateString(source), + _timeEvent, + setArmControlState, + setArmControlState? armControlState:0, + setArmConfiguration, + setArmConfiguration? armConfiguration:0, + hasMonitorState, + hasMonitorState ? monitorState:0, + hasMonitorConfig, + monitorConfig, + FRIMessage + ); +} +/// KUKAiiwa.fbs +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const std::vector> &kukaiiwastates) +{ + return grl::flatbuffer::CreateKUKAiiwaStates( + fbb, + fbb.CreateVector>(kukaiiwastates)); +} + + +/// Helper function for use when building up messages to save to a log file. +/// Call this just before SaveFlatBufferFile. See fusionTrackExample for how +/// and when to use it. +bool FinishAndVerifyBuffer( + flatbuffers::FlatBufferBuilder& fbb, + std::vector>& KUKAiiwaState_vector +) +{ + + auto states = fbb.CreateVector(KUKAiiwaState_vector); + auto fbLogKUKAiiwaStates = grl::flatbuffer::CreateKUKAiiwaStates(fbb, states); + + // Finish a buffer with given object + // Call `Finish()` to instruct the builder fbb that this frame is complete. + const char *file_identifier = grl::flatbuffer::KUKAiiwaStatesIdentifier(); + + fbb.Finish(fbLogKUKAiiwaStates, file_identifier); + + flatbuffers::uoffset_t _max_depth = 64; + flatbuffers::uoffset_t _max_tables = 1000000000; + auto verifier = flatbuffers::Verifier(fbb.GetBufferPointer(), fbb.GetSize(), _max_depth, _max_tables); + bool success = grl::flatbuffer::VerifyKUKAiiwaStatesBuffer(verifier); + assert(success && "VerifyKUKAiiwaStatesBuffer"); + + return success; +} +} // End of grl namespace + + +#endif // GRL_ATRACSYS_FUSION_TRACK_TO_FLATBUFFER diff --git a/include/grl/ros/KukaLBRiiwaROSPlugin.hpp b/include/grl/ros/KukaLBRiiwaROSPlugin.hpp index 770abc7..682e2af 100644 --- a/include/grl/ros/KukaLBRiiwaROSPlugin.hpp +++ b/include/grl/ros/KukaLBRiiwaROSPlugin.hpp @@ -364,7 +364,9 @@ namespace grl { if(haveNewData) { std::vector wrench_vector; - KukaDriverP_->getWrench(std::back_inserter(wrench_vector)); + // KukaDriverP_->getWrench(std::back_inserter(wrench_vector)); + // Using Container instead of OutputIterator to write into data + KukaDriverP_->getWrench(wrench_vector); if (!wrench_vector.empty()) { current_wrench.force.x = wrench_vector[0]; @@ -403,7 +405,7 @@ namespace grl { bool debug; std::size_t iteration_count_ = 0; - + grl::flatbuffer::ArmState interaction_mode; boost::mutex jt_mutex; diff --git a/include/grl/sensor/FusionTrack.hpp b/include/grl/sensor/FusionTrack.hpp index 6d7c6a5..1e4eaea 100644 --- a/include/grl/sensor/FusionTrack.hpp +++ b/include/grl/sensor/FusionTrack.hpp @@ -181,6 +181,8 @@ class FusionTrack std::vector geometries; geometries.push_back("geometry0022.ini"); geometries.push_back("geometry0055.ini"); + // Add the new marker attached to the frame + geometries.push_back("geometry50000.ini"); params.geometryFilenames = geometries; return params; } @@ -208,8 +210,7 @@ class FusionTrack cartographer::common::Time FusionTrackTimeToCommonTime(typename MicrosecondClock::time_point FTtime) { return cartographer::common::Time( - std::chrono::duration_cast(FTtime.time_since_epoch()) + - std::chrono::seconds(cartographer::common::kUtsEpochOffsetFromUnixEpochInSeconds)); + std::chrono::duration_cast(FTtime.time_since_epoch())); } cartographer::common::Time ImageHeaderToCommonTime(const ::ftkImageHeader &tq) @@ -282,6 +283,7 @@ class FusionTrack // a mutex is not required because this was a mistake, it is actually required. ftkClose(&m_ftkLibrary); m_ftkLibrary = nullptr; + std::this_thread::sleep_for(std::chrono::milliseconds(100)); } } @@ -662,15 +664,18 @@ class FusionTrack std::string s_event_name = ss_event_name.str(); std::string device_clock_id_str = s_event_name + m_params.deviceClockID; TimeEvent::UnsignedCharArray event_name; - s_event_name.copy(event_name.begin(), std::min(event_name.size(), s_event_name.size())); + std::size_t length = s_event_name.copy(event_name.begin(), std::min(event_name.size(), s_event_name.size())); + event_name[length] = '\0'; m_event_names.push_back(event_name); TimeEvent::UnsignedCharArray device_clock_id; - device_clock_id_str.copy(device_clock_id.begin(), std::min(device_clock_id_str.size(), device_clock_id.size())); + length = device_clock_id_str.copy(device_clock_id.begin(), std::min(device_clock_id_str.size(), device_clock_id.size())); + device_clock_id[length] = '\0'; m_device_clock_ids.push_back(device_clock_id); TimeEvent::UnsignedCharArray local_clock_name_arr; - m_params.localClockID.copy(local_clock_name_arr.begin(), std::min(local_clock_name_arr.size(), m_params.localClockID.size())); + length = m_params.localClockID.copy(local_clock_name_arr.begin(), std::min(local_clock_name_arr.size(), m_params.localClockID.size())); + local_clock_name_arr[length] = '\0'; m_local_clock_ids.push_back(local_clock_name_arr); } diff --git a/include/grl/sensor/FusionTrackLogAndTrack.hpp b/include/grl/sensor/FusionTrackLogAndTrack.hpp index ef2276a..ef5f593 100644 --- a/include/grl/sensor/FusionTrackLogAndTrack.hpp +++ b/include/grl/sensor/FusionTrackLogAndTrack.hpp @@ -244,7 +244,7 @@ class FusionTrackLogAndTrack : public std::enable_shared_from_thisMarkers.size() << std::endl; for(const auto &marker : m_receivedFrame->Markers) { - // std::cout << "marker geometryid: " << marker.geometryId << std::endl; + // std::cout << "marker geometryid: " << marker.geometryId << std::endl; cameraToMarkerTransform = sensor::ftkMarkerToAffine3f(marker); auto configIterator = m_geometryIDToMotionConfigParams.find(marker.geometryId); - if (configIterator == m_geometryIDToMotionConfigParams.end()) continue; // no configuration for this item + if (configIterator == m_geometryIDToMotionConfigParams.end()) { + // std::cout << "marker geometryid: " << marker.geometryId << " NOT found config!" << std::endl; + continue; // no configuration for this item + } auto config = configIterator->second; - // std::cout << "<<<<<<<<<<<<<<<< found config!" << std::endl; + // invert the transform from the tracker to the object if needed if (m_opticalTrackerBase == std::get(config) && @@ -329,6 +335,7 @@ class FusionTrackLogAndTrack : public std::enable_shared_from_this(config), std::get(config), cameraToMarkerTransform)); } + // std::cout << "Pose size: " << poses.size() << std::endl; return poses; } @@ -339,9 +346,10 @@ class FusionTrackLogAndTrack : public std::enable_shared_from_thisinfo("Save Recording as: ", filename); -#else // HAVE_spdlog - std::cout << "Save Recording as: " << filename << std::endl; -#endif // HAVE_spdlog + #ifdef HAVE_spdlog + loggerP->info("Save Recording as: {}", filename); + #else // HAVE_spdlog + std::cout << "Save Recording as: {}" << filename << std::endl; + #endif // HAVE_spdlog /// lock mutex before accessing file std::lock_guard lock(m_frameAccess); @@ -391,9 +401,9 @@ class FusionTrackLogAndTrack : public std::enable_shared_from_this(save_fbbP->GetBufferPointer()), save_fbbP->GetSize(), write_binary_stream); - /// TODO(ahundt) replace cout with proper spdlog and vrep banner notification + /// TODO(ahFusionTrackLogAndTrackundt) replace cout with proper spdlog and vrep banner notification #ifdef HAVE_spdlog - lambdaLoggerP->info("filename: ", filename, " verifier success: ", success); + lambdaLoggerP->info("For FT filename: {}, verifier success:{}", filename,success); #else // HAVE_spdlog std::cout << "filename: " << filename << " verifier success: " << success << std::endl; #endif // HAVE_spdlog @@ -437,9 +447,7 @@ class FusionTrackLogAndTrack : public std::enable_shared_from_this deviceSerialNumbers; try { @@ -460,7 +468,7 @@ class FusionTrackLogAndTrack : public std::enable_shared_from_this lock(m_frameAccess); if (m_isRecording) { - // convert the buffer into a flatbuffer for recording and add it to the in memory buffer - // @todo TODO(ahundt) if there haven't been problems, delete this todo, but if recording in the driver thread is time consuming move the code to another thread - if (!m_logFileBufferBuilderP) - { - // flatbuffersbuilder does not yet exist - m_logFileBufferBuilderP = std::make_shared(); - m_KUKAiiwaFusionTrackMessageBufferP = - std::make_shared>>(); - } - BOOST_VERIFY(m_logFileBufferBuilderP != nullptr); - BOOST_VERIFY(opticalTrackerP != nullptr); - BOOST_VERIFY(m_nextState != nullptr); - - if(m_nextState->Error != FTK_WAR_NO_FRAME) - { - // only collect frames actually containing new data. - flatbuffers::Offset oneKUKAiiwaFusionTrackMessage = - grl::toFlatBuffer(*m_logFileBufferBuilderP, *opticalTrackerP, *m_nextState); - m_KUKAiiwaFusionTrackMessageBufferP->push_back(oneKUKAiiwaFusionTrackMessage); - } - - // There is a flatbuffers file size limit of 2GB, but we use a conservative 512MB - if(m_logFileBufferBuilderP->GetSize() > single_buffer_limit_bytes) - { - // save the file if we are over the limit - saveFileNow = true; - } + // convert the buffer into a flatbuffer for recording and add it to the in memory buffer + // @todo TODO(ahundt) if there haven't been problems, delete this todo, but if recording in the driver thread is time consuming move the code to another thread + if (!m_logFileBufferBuilderP) + { + // flatbuffersbuilder does not yet exist + m_logFileBufferBuilderP = std::make_shared(); + m_KUKAiiwaFusionTrackMessageBufferP = + std::make_shared>>(); + } + BOOST_VERIFY(m_logFileBufferBuilderP != nullptr); + BOOST_VERIFY(opticalTrackerP != nullptr); + BOOST_VERIFY(m_nextState != nullptr); + + if(m_nextState->Error != FTK_WAR_NO_FRAME) + { + // only collect frames actually containing new data. + flatbuffers::Offset oneKUKAiiwaFusionTrackMessage = + grl::toFlatBuffer(*m_logFileBufferBuilderP, *opticalTrackerP, *m_nextState); + m_KUKAiiwaFusionTrackMessageBufferP->push_back(oneKUKAiiwaFusionTrackMessage); + } + + // There is a flatbuffers file size limit of 2GB, but we use a conservative 512MB + if(m_logFileBufferBuilderP->GetSize() > single_buffer_limit_bytes ) + // || m_KUKAiiwaFusionTrackMessageBufferP->size() > single_buffer_limit_messages) + { + // save the file if we are over the limit + saveFileNow = true; + } } // end recording steps if(m_nextState->Error == FTK_OK) @@ -515,8 +524,8 @@ class FusionTrackLogAndTrack : public std::enable_shared_from_this loggerP; #endif // HAVE_spdlog diff --git a/include/grl/sensor/FusionTrackToFlatbuffer.hpp b/include/grl/sensor/FusionTrackToFlatbuffer.hpp index 896606c..77ae721 100644 --- a/include/grl/sensor/FusionTrackToFlatbuffer.hpp +++ b/include/grl/sensor/FusionTrackToFlatbuffer.hpp @@ -1,11 +1,16 @@ #ifndef GRL_ATRACSYS_FUSION_TRACK_TO_FLATBUFFER #define GRL_ATRACSYS_FUSION_TRACK_TO_FLATBUFFER +/// Before including any FlatBuffers related headers, you can add this #define. +/// You'll get an assert whenever the verifier fails, whose stack-trace can tell you exactly what check failed an on what field etc. +#define FLATBUFFERS_DEBUG_VERIFICATION_FAILURE + #include "FusionTrackToEigen.hpp" #include "FusionTrack.hpp" #include "grl/flatbuffer/FusionTrack_generated.h" #include "grl/flatbuffer/Time_generated.h" #include "grl/flatbuffer/LogKUKAiiwaFusionTrack_generated.h" +#include "grl/flatbuffer/HelperToFlatbuffer.hpp" #include #include #include @@ -35,8 +40,11 @@ bool FinishAndVerifyBuffer( // fbb.Finish(oneKUKAiiwaFusionTrackMessage, file_identifier); fbb.Finish(fbLogKUKAiiwaFusionTrack, file_identifier); - auto verifier = flatbuffers::Verifier(fbb.GetBufferPointer(), fbb.GetSize()); + flatbuffers::uoffset_t _max_depth = 64; + flatbuffers::uoffset_t _max_tables = 1000000000; + auto verifier = flatbuffers::Verifier(fbb.GetBufferPointer(), fbb.GetSize(), _max_depth, _max_tables); bool success = grl::flatbuffer::VerifyLogKUKAiiwaFusionTrackBuffer(verifier); + assert(success && "VerifyLogKUKAiiwaFusionTrackBuffer"); return success; } @@ -46,28 +54,47 @@ grl::flatbuffer::Vector3d toFlatBuffer(const ::ftk3DPoint &pt) return grl::flatbuffer::Vector3d(pt.x, pt.y, pt.z); } -grl::flatbuffer::Vector3d toFlatBuffer(const Eigen::Vector3d &pt) -{ - return grl::flatbuffer::Vector3d(pt.x(), pt.y(), pt.z()); -} - -grl::flatbuffer::Quaternion toFlatBuffer(Eigen::Quaterniond q) -{ - return grl::flatbuffer::Quaternion(q.x(), q.y(), q.z(), q.w()); -} - -grl::flatbuffer::Pose toFlatBuffer(Eigen::Affine3d tf) -{ - Eigen::Vector3d pos = tf.translation(); - Eigen::Quaterniond eigenQuat(tf.rotation()); - return grl::flatbuffer::Pose(toFlatBuffer(pos), toFlatBuffer(eigenQuat)); +/// the second parameter is merely a tag to uniquely identify this function so it compiles +/// its value is not utilized or modified. +grl::flatbuffer::ftkQueryStatus toFlatBuffer(const ::ftkQueryStatus queryStatus) { + + switch(queryStatus) { + + case QS_OK: + return grl::flatbuffer::ftkQueryStatus::QS_OK; + case QS_ERR_OVERFLOW: + return grl::flatbuffer::ftkQueryStatus::QS_ERR_OVERFLOW; + case QS_ERR_INVALID_RESERVED_SIZE: + return grl::flatbuffer::ftkQueryStatus::QS_ERR_INVALID_RESERVED_SIZE; + case QS_REPROCESS: + return grl::flatbuffer::ftkQueryStatus::QS_REPROCESS; + default: + break; + } + return grl::flatbuffer::ftkQueryStatus::QS_WAR_SKIPPED; } -grl::flatbuffer::Pose toFlatBuffer(Eigen::Affine3f tf) -{ - return toFlatBuffer(tf.cast()); +/// the second parameter is merely a tag to identify this function so it compiles +/// its value is not utilized or modified. +grl::flatbuffer::ftkDeviceType toFlatBuffer(const ::ftkDeviceType deviceType, grl::flatbuffer::ftkDeviceType tag) { +// grl::flatbuffer::ftkDeviceType toFlatBuffer(const ::ftkDeviceType deviceType) { + switch(deviceType) { + case DEV_SIMULATOR: /*!< Internal use only */ + return grl::flatbuffer::ftkDeviceType::DEV_SIMULATOR; + + case DEV_INFINITRACK: /*!< Device is an infiniTrack */ + return grl::flatbuffer::ftkDeviceType::DEV_INFINITRACK; + + case DEV_FUSIONTRACK_500: /*!< Device is a fusionTrack 500 */ + return grl::flatbuffer::ftkDeviceType::DEV_FUSIONTRACK_500; + + case DEV_FUSIONTRACK_250: /*!< Device is a fusionTrack 250 */ + return grl::flatbuffer::ftkDeviceType::DEV_FUSIONTRACK_250; + default: /**< Unknown device type. */ + break; + }; + return grl::flatbuffer::ftkDeviceType::DEV_UNKNOWN_DEVICE; } - flatbuffers::Offset toFlatBuffer(flatbuffers::FlatBufferBuilder &fbb, const ::ftkGeometry &geometry, @@ -93,7 +120,6 @@ toFlatBuffer(flatbuffers::FlatBufferBuilder &fbb, _fbb.Finish(_ftkGeometry); auto verifier = flatbuffers::Verifier(_fbb.GetBufferPointer(), _fbb.GetSize()); bool success = verifier.VerifyBuffer(); - std::cout <<" verifier success for ftkGeometry: " << success << std::endl; /////////////////////////////////////////////////////////////////////////// return grl::flatbuffer::CreateftkGeometry( @@ -280,8 +306,8 @@ toFlatBuffer(flatbuffers::FlatBufferBuilder &fbb, flatbuffers::Offset toFlatBuffer(flatbuffers::FlatBufferBuilder &fbb, const grl::sensor::FusionTrack::Frame &frame, - bool writeRegionOfInterest = true, - bool writeFiducials = true, + bool writeRegionOfInterest = false, + bool writeFiducials = false, bool writeMarkers = true) { /// @todo TODO(ahundt) IN PROGRESS Here we should get the markers'name @@ -315,10 +341,12 @@ toFlatBuffer(flatbuffers::FlatBufferBuilder &fbb, uint32_t height = frame.imageHeader.height; int32_t imageStrideInBytes = frame.imageHeader.imageStrideInBytes; uint32_t imageHeaderVersion = frame.FrameQueryP->imageHeaderVersionSize.Version; + int32_t imageHeaderStatus = frame.FrameQueryP->imageHeaderStat; flatbuffers::Offset imageLeftPixels = frame.CameraImageLeftP ? fbb.CreateString(reinterpret_cast(frame.CameraImageLeftP->begin()), sizeof(frame.CameraImageLeftP)) : 0; uint32_t imageLeftPixelsVersion = frame.FrameQueryP->imageLeftVersionSize.Version; int32_t imageLeftStatus = frame.FrameQueryP->imageLeftStat; + flatbuffers::Offset imageRightPixels = frame.CameraImageRightP ? fbb.CreateString(reinterpret_cast(frame.CameraImageRightP->begin()), sizeof(frame.CameraImageRightP)) : 0; uint32_t imageRightPixelsVersion = frame.FrameQueryP->imageRightVersionSize.Version; int32_t imageRightStatus = frame.FrameQueryP->imageRightStat; @@ -331,6 +359,7 @@ toFlatBuffer(flatbuffers::FlatBufferBuilder &fbb, (writeRegionOfInterest && frame.ImageRegionOfInterestBoxesRight.size()) ? toFlatBuffer(fbb, frame.ImageRegionOfInterestBoxesRight): 0; uint32_t regionsOfInterestRightVersion = frame.FrameQueryP->rawDataRightVersionSize.Version; int32_t regionsOfInterestRightStatus = frame.FrameQueryP->rawDataRightStat; + flatbuffers::Offset>> threeDFiducials = (writeFiducials && frame.Fiducials.size()) ? toFlatBuffer(fbb, frame.Fiducials, fiducialIndexToMarkerIDs) : 0; uint32_t threeDFiducialsVersion = frame.FrameQueryP->threeDFiducialsVersionSize.Version; @@ -412,50 +441,13 @@ toFlatBuffer(flatbuffers::FlatBufferBuilder &fbb, fb_m_device_types); } -template -typename T::value_type stringLength(const T &array) -{ - - auto iter = std::find(array.begin(), array.end(), '\0'); - auto len = std::distance(array.begin(), iter); - return len; -} - -flatbuffers::Offset -toFlatBuffer(flatbuffers::FlatBufferBuilder &fbb, - const grl::TimeEvent &timeStamp) -{ - flatbuffers::Offset event_name = fbb.CreateString(const_cast(timeStamp.event_name.begin()), stringLength(timeStamp.event_name)); - /// https://github.com/googlecartographer/cartographer/blob/master/cartographer/common/time.cc - /// convert time to int64 - int64_t local_request_time = cartographer::common::ToUniversal(timeStamp.local_request_time); - flatbuffers::Offset device_clock_id = fbb.CreateString(const_cast(timeStamp.device_clock_id.begin()), stringLength(timeStamp.device_clock_id)); - int64_t device_time = cartographer::common::ToUniversal(timeStamp.device_time); - flatbuffers::Offset local_clock_id = fbb.CreateString(const_cast(timeStamp.local_clock_id.begin()), stringLength(timeStamp.local_clock_id)); - int64_t local_receive_time = cartographer::common::ToUniversal(timeStamp.local_receive_time); - int64_t corrected_local_time = cartographer::common::ToUniversal(timeStamp.corrected_local_time); - int64_t clock_skew = cartographer::common::ToSeconds(timeStamp.clock_skew); - int64_t min_transport_delay = cartographer::common::ToSeconds(timeStamp.min_transport_delay); - return grl::flatbuffer::CreateTimeEvent( - fbb, - event_name, - local_request_time, - device_clock_id, - device_time, - local_clock_id, - local_receive_time, - corrected_local_time, - clock_skew, - min_transport_delay); -} - flatbuffers::Offset toFlatBuffer(flatbuffers::FlatBufferBuilder &fbb, const grl::sensor::FusionTrack &fusiontrack, const grl::sensor::FusionTrack::Frame &frame, - bool writeParameters = true, - bool writeRegionOfInterest = true, - bool writeFiducials = true, + bool writeParameters = false, + bool writeRegionOfInterest = false, + bool writeFiducials = false, bool writeMarkers = true) { static const double microsecToSec = 1 / 1000000; diff --git a/include/grl/vrep/Eigen.hpp b/include/grl/vrep/Eigen.hpp index 2facfea..92e430c 100644 --- a/include/grl/vrep/Eigen.hpp +++ b/include/grl/vrep/Eigen.hpp @@ -3,6 +3,9 @@ #ifndef _VREP_EIGEN_HPP_ #define _VREP_EIGEN_HPP_ +/// BOOST_THROW_EXCEPTION +#include + #include #include #include @@ -23,7 +26,7 @@ Eigen::Quaterniond vrepToEigenQuaternion(InputIterator vrepQuat){ // 0123 // vrep is ordered xyzw, // eigen is ordered wxyz - + Eigen::Quaterniond quat(vrepQuat[vrepquat::w],vrepQuat[vrepquat::x],vrepQuat[vrepquat::y],vrepQuat[vrepquat::z]); return quat; } @@ -31,7 +34,7 @@ Eigen::Quaterniond vrepToEigenQuaternion(InputIterator vrepQuat){ template std::array EigenToVrepQuaternion(const Q& q){ std::array qa; - + // 0123 // vrep is ordered xyzw, // eigen is ordered wxyz @@ -39,7 +42,7 @@ std::array EigenToVrepQuaternion(const Q& q){ qa[vrepquat::y] = q.y(); qa[vrepquat::z] = q.z(); qa[vrepquat::w] = q.w(); - + return qa; } @@ -49,7 +52,7 @@ std::array EigenToVrepPosition(const P& p){ qv[0] = p.x(); qv[1] = p.y(); qv[2] = p.z(); - + return qv; } @@ -94,7 +97,7 @@ Eigen::Affine3d vrepToEigenTransform(const std::array& vrepTransform) std::pair getAxisAngleAndTranslation(int ObjectHandle, int BaseFrameObjectHandle){ - + std::array simTipPosition; //std::array simTipOrientation; std::array simTipQuaternion; @@ -103,12 +106,12 @@ std::pair getAxisAngleAndTranslation(int Object if(ret==-1) BOOST_THROW_EXCEPTION(std::runtime_error("HandEyeCalibrationVrepPlugin: Could not get position")); ret = simGetObjectQuaternion(ObjectHandle, BaseFrameObjectHandle, simTipQuaternion.begin()); if(ret==-1) BOOST_THROW_EXCEPTION(std::runtime_error("HandEyeCalibrationVrepPlugin: Could not get quaternion")); - + auto simTipAngleAxis = vrepQuatToEigenVector3dAngleAxis(simTipQuaternion.begin()); auto simTipVec = vrepToEigenVector3d(simTipPosition.begin()); - - - + + + return std::make_pair(simTipAngleAxis,simTipVec); } @@ -121,11 +124,11 @@ void setObjectTransform(int objectHandle, int relativeToObjectHandle, T& transfo std::array vrepQuat = EigenToVrepQuaternion(eigenQuat); int ret = simSetObjectQuaternion(objectHandle,relativeToObjectHandle,vrepQuat.begin()); if(ret==-1) BOOST_THROW_EXCEPTION(std::runtime_error("setObjectTransform: Could not set Quaternion")); - + std::array vrepPos = EigenToVrepPosition(transform.translation()); ret = simSetObjectPosition(objectHandle,relativeToObjectHandle,vrepPos.begin()); if(ret==-1) BOOST_THROW_EXCEPTION(std::runtime_error("setObjectTransform: Could not set Quaternion")); - + } /// @param objectHandle The V-Rep object handle for which the transform is needed @@ -138,17 +141,17 @@ Eigen::Affine3d getObjectTransform(int objectHandle, int relativeToObjectHandle int ret = simGetObjectQuaternion(objectHandle,relativeToObjectHandle,vrepQuat.begin()); Eigen::Quaterniond eigenQuat(vrepToEigenQuaternion(vrepQuat)); if(ret==-1) BOOST_THROW_EXCEPTION(std::runtime_error("getObjectTransform: Could not get Quaternion")); - + std::array vrepPos; ret = simGetObjectPosition(objectHandle,relativeToObjectHandle,vrepPos.begin()); if(ret==-1) BOOST_THROW_EXCEPTION(std::runtime_error("getObjectTransform: Could not get Position")); Eigen::Vector3d eigenPos(vrepToEigenVector3d(vrepPos)); - + Eigen::Affine3d transform; - + transform = eigenQuat; transform.translation() = eigenPos; - + return transform; } @@ -163,12 +166,12 @@ std::pair getObjectTransformQuaternionTransl int ret = simGetObjectQuaternion(objectHandle,relativeToObjectHandle,vrepQuat.begin()); if(ret==-1) BOOST_THROW_EXCEPTION(std::runtime_error("getObjectTransformQuaternionTranslationPair: Could not get Quaternion")); Eigen::Quaterniond eigenQuat(vrepToEigenQuaternion(vrepQuat)); - + std::array vrepPos; ret = simGetObjectPosition(objectHandle,relativeToObjectHandle,vrepPos.begin()); if(ret==-1) BOOST_THROW_EXCEPTION(std::runtime_error("getObjectTransformQuaternionTranslationPair: Could not get Position")); Eigen::Vector3d eigenPos(vrepToEigenVector3d(vrepPos)); - + return std::make_pair(eigenQuat,eigenPos); } diff --git a/include/grl/vrep/HandEyeCalibrationVrepPlugin.hpp b/include/grl/vrep/HandEyeCalibrationVrepPlugin.hpp index 40fe622..a7789ff 100644 --- a/include/grl/vrep/HandEyeCalibrationVrepPlugin.hpp +++ b/include/grl/vrep/HandEyeCalibrationVrepPlugin.hpp @@ -9,11 +9,18 @@ #include "grl/vrep/Eigen.hpp" #include "grl/vrep/Vrep.hpp" +#include "grl/time.hpp" #include "camodocal/calib/HandEyeCalibration.h" #include "v_repLib.h" #include "grl/vector_ostream.hpp" +// boost::filesystem +#include +#include +#include +#include + namespace grl { @@ -83,7 +90,7 @@ void construct(){ void addFrame() { - logger_->info( "Adding hand eye calibration frame #", ++frameCount); + logger_->info( "Adding hand eye calibration frame {}", ++frameCount); auto robotTipInRobotBase = getObjectTransform(robotTip,robotBase); auto fiducialInOpticalTrackerBase = getObjectTransform(opticalTrackerDetectedObjectName,opticalTrackerBase); @@ -94,30 +101,46 @@ void addFrame() { isFirstFrame = false; } - auto robotTipInFirstTipBase = firstRobotTipInRobotBaseInverse * robotTipInRobotBase; // A_0_Inv * A_i - auto fiducialInFirstFiducialBase = firstFiducialInOpticalTrackerBaseInverse * fiducialInOpticalTrackerBase; // B_0_Inv * B_i + auto robotTipInFirstTipBase = firstRobotTipInRobotBaseInverse * robotTipInRobotBase; // B_0_Inv * B_i + auto fiducialInFirstFiducialBase = firstFiducialInOpticalTrackerBaseInverse * fiducialInOpticalTrackerBase; // A_0_Inv * A_i + auto rvec1 = eigenRotToEigenVector3dAngleAxis(robotTipInFirstTipBase.rotation()); + auto tvec1 = robotTipInFirstTipBase.translation(); + auto rvec2 = eigenRotToEigenVector3dAngleAxis(fiducialInFirstFiducialBase.rotation()); + auto tvec2 = fiducialInFirstFiducialBase.translation(); - rvecsArm.push_back( eigenRotToEigenVector3dAngleAxis(robotTipInFirstTipBase.rotation() )); - tvecsArm.push_back( robotTipInFirstTipBase.translation() ); + if (rvec1.norm() != 0 && rvec2.norm() != 0) { - rvecsFiducial.push_back(eigenRotToEigenVector3dAngleAxis(fiducialInFirstFiducialBase.rotation() )); - tvecsFiducial.push_back( fiducialInFirstFiducialBase.translation()); + rvecsArm.push_back(rvec1); + tvecsArm.push_back(tvec1); + + rvecsFiducial.push_back(rvec2); + tvecsFiducial.push_back(tvec2); + } else { + std::cout << "Empty Vector" << std::endl; + std::cout << rvec1 << std::endl; + std::cout << rvec2 << std::endl; + } if(debug){ - logger_->info( "\nrobotTipInRobotBase:\n", poseString(robotTipInRobotBase)); - logger_->info( "\nfiducialInOpticalTrackerBase:\n", poseString(fiducialInOpticalTrackerBase)); + std::cout << "rvec1: " << std::endl << rvec1 << std::endl; + std::cout << "tvec1: " << std::endl << tvec1 << std::endl; + std::cout << "rvec2: " << std::endl << rvec2 << std::endl; + std::cout << "tvec2: " << std::endl << tvec2 << std::endl; - logger_->info( "\nrobotTipInFirstTipBase:\n", poseString(robotTipInFirstTipBase)); - logger_->info( "\nfiducialInFirstFiducialBase:\n", poseString(fiducialInFirstFiducialBase)); + logger_->info( "\nrobotTipInRobotBase: \n{}", poseString(robotTipInRobotBase)); + logger_->info( "\nfiducialInOpticalTrackerBase: \n{}", poseString(fiducialInOpticalTrackerBase)); - // print simulation transfrom from tip to fiducial - Eigen::Affine3d RobotTipToFiducial = getObjectTransform(opticalTrackerDetectedObjectName,robotTip); - logger_->info( poseString(RobotTipToFiducial,"expected RobotTipToFiducial (simulation only): ")); + logger_->info( "\nrobotTipInFirstTipBase: \n{}", poseString(robotTipInFirstTipBase)); + logger_->info( "\nfiducialInFirstFiducialBase: \n{}", poseString(fiducialInFirstFiducialBase)); + + // print simulation transfrom from tip to fiducial + Eigen::Affine3d RobotTipToFiducial = getObjectTransform(opticalTrackerDetectedObjectName,robotTip); + logger_->info( poseString(RobotTipToFiducial,"expected RobotTipToFiducial (simulation only): ")); - BOOST_VERIFY(robotTipInFirstTipBase.translation().norm() - fiducialInFirstFiducialBase.translation().norm() < 0.1); + //BOOST_VERIFY(robotTipInFirstTipBase.translation().norm() - fiducialInFirstFiducialBase.translation().norm() < 0.1); } } @@ -129,8 +152,8 @@ void addFrame() { /// @todo evaluate if applyEstimate should not be called by this void estimateHandEyeScrew(){ - logger_->info( "Running Hand Eye Screw Estimate with the following numbers of entries in each category: rvecsFiducial",rvecsFiducial.size(), - " tvecsFiducial: ", tvecsFiducial.size(), " rvecsArm: ", rvecsArm.size(), " tvecsArm: ", tvecsArm.size()); + logger_->info( "Running Hand Eye Screw Estimate with the following numbers of entries in each category: rvecsFiducial {} , tvecsFiducial: {} , rvecsArm: {} , tvecsArm: {}" + ,rvecsFiducial.size(), tvecsFiducial.size(), rvecsArm.size(), tvecsArm.size()); BOOST_VERIFY(allHandlesSet); @@ -140,7 +163,8 @@ void estimateHandEyeScrew(){ tvecsArm, rvecsFiducial, tvecsFiducial, - transformEstimate.matrix() + transformEstimate.matrix(), + false //PlanarMotion ); @@ -153,29 +177,60 @@ void estimateHandEyeScrew(){ if(debug){ // print simulation transfrom from tip to fiducial Eigen::Affine3d RobotTipToFiducial = getObjectTransform(opticalTrackerDetectedObjectName,robotTip); - logger_->info( "\n", poseString(RobotTipToFiducial,"expected RobotTipToFiducial (simulation only): ")); + logger_->info( "\n{}", poseString(RobotTipToFiducial,"expected RobotTipToFiducial (simulation only): ")); } - logger_->info( "\n", poseString(transformEstimate,"estimated RobotTipToFiducial:")); + + + + logger_->info( "\n{}", poseString(transformEstimate,"estimated RobotTipToFiducial:")); applyEstimate(); // print results + Eigen::Quaterniond eigenQuat(transformEstimate.rotation()); - logger_->info( "Hand Eye Screw Estimate quat wxyz\n: ", eigenQuat.w(), " ", eigenQuat.x(), " ", eigenQuat.y(), " ", eigenQuat.z(), " ", " translation xyz: ", transformEstimate.translation().x(), " ", transformEstimate.translation().y(), " ", transformEstimate.translation().z(), " "); - - logger_->info( "Optical Tracker Base Measured quat wxyz\n: ", detectedObjectQuaternion[0], " ", detectedObjectQuaternion[1], " ", detectedObjectQuaternion[2], " ", detectedObjectQuaternion[3], " ", " translation xyz: ", detectedObjectPosition[0], " ", detectedObjectPosition[1], " ", detectedObjectPosition[2], " "); - + logger_->info( "Hand Eye Screw Estimate quat wxyz\n: {} , {} , {} , {} \n translation xyz: {} {} {}", + eigenQuat.w(), eigenQuat.x(), eigenQuat.y(),eigenQuat.z(), transformEstimate.translation().x(), transformEstimate.translation().y(), transformEstimate.translation().z()); + + logger_->info( "Optical Tracker Base Measured quat wxyz\n: {} , {} , {} , {} \n translation xyz: {} , {}, {}", + detectedObjectQuaternion[0], detectedObjectQuaternion[1], detectedObjectQuaternion[2], detectedObjectQuaternion[3], + detectedObjectPosition[0], detectedObjectPosition[1], detectedObjectPosition[2]); + + + // Write the hand eye calibration results into a file + auto myFile = boost::filesystem::current_path() /"HandEyeCalibration_Result.txt"; + boost::filesystem::ofstream ofs(myFile, std::ios_base::app); + // boost::archive::text_oarchive ta(ofs); + ofs <<"\n\n=========== " + current_date_and_time_string() + " =======================================\n"; + ofs << "Hand Eye Screw Estimate quat wxyz: " << eigenQuat.w() << ", "<< eigenQuat.x() << ", " << eigenQuat.y() << ", " << eigenQuat.z() + << "\nTranslation xyz: " << transformEstimate.translation().x() << ", " << transformEstimate.translation().y() << ", " << transformEstimate.translation().z() + << "\n\nestimated RobotTipToFiducial: \n" << poseString(transformEstimate); + ofs.close(); + if ( boost::filesystem::exists( myFile )) { + std::cout << "Hand eye calibration result has been writen into " << myFile.string() << std::endl; + } + } /// @brief Will apply the stored estimate to the v-rep simulation value /// /// A default transform is saved when construct() was called /// so if no estimate has been found that will be used. -void applyEstimate(){ +void applyEstimate(std::string sceneName=""){ // set transform between end effector and fiducial setObjectTransform(opticalTrackerDetectedObjectName, robotTip, transformEstimate); + if(!sceneName.empty()){ + int ret = simSaveScene(sceneName.c_str()); + if(ret == -1){ + logger_->info( "\n{} fails to be saved...", sceneName); + } else { + logger_->info( "\n{} has been saved...", sceneName); + } + + } + } Eigen::Affine3d getEstimate(){ diff --git a/include/grl/vrep/InverseKinematicsVrepPlugin.hpp b/include/grl/vrep/InverseKinematicsVrepPlugin.hpp index 0f78f16..3510071 100644 --- a/include/grl/vrep/InverseKinematicsVrepPlugin.hpp +++ b/include/grl/vrep/InverseKinematicsVrepPlugin.hpp @@ -5,6 +5,30 @@ /// @todo remove IGNORE_ROTATION or make it a runtime configurable parameter // #define IGNORE_ROTATION +#include +#include +#include +#include +#include + +#include "flatbuffers/util.h" +#include "grl/flatbuffer/ParseflatbuffertoCSV.hpp" +#include +#include "grl/flatbuffer/FusionTrack_generated.h" +#include "grl/flatbuffer/Time_generated.h" +#include "grl/flatbuffer/LogKUKAiiwaFusionTrack_generated.h" +#include "grl/time.hpp" + + +#include +#include +#include +#include + +#include +#include +/// Boost to create an empty folder +#include #include @@ -52,6 +76,28 @@ #include +// boost::filesystem +#include +#include +#include +#include + + +// FusionTrack Libraries +// #include +// For replay the recorded data +// #include "grl/flatbuffer/ParseflatbuffertoCSV.hpp" +#include + #include + + // mc_rbdyn_urdf +// https://github.com/jrl-umi3218/mc_rbdyn_urdf +#include +#include "grl/flatbuffer/kukaiiwaURDF.h" +#include "grl/flatbuffer/CSVIterator.hpp" + +#include + namespace grl { namespace vrep { @@ -82,7 +128,7 @@ void SetVRepArmFromRBDyn( // simSetJointTargetVelocity(jointHandles_[i],jointAngles_dt[i]/simulationTimeStep); // simSetJointTargetPosition(jointHandles_[i],jointAngles_dt[i]); // simSetJointTargetPosition(jointHandles_[i],futureAngle); - + if(!debug.empty()) { str+=boost::lexical_cast(futureAngle); @@ -122,12 +168,12 @@ void SetRBDynArmFromVrep( simGetJointPosition(vrepJointHandles[i],&futureAngle); /// @todo TODO(ahundt) warn/error when size!=0 or explicitly handle those cases if(simArmConfig.q[jointIdx].size()>0) simArmConfig.q[jointIdx][0] = futureAngle; - + /// @todo TODO(ahundt) add torque information // simSetJointTargetVelocity(jointHandles_[i],jointAngles_dt[i]/simulationTimeStep); // simSetJointTargetPosition(jointHandles_[i],jointAngles_dt[i]); // simSetJointTargetPosition(jointHandles_[i],futureAngle); - + if(!debug.empty()) { str+=boost::lexical_cast(futureAngle); @@ -159,38 +205,37 @@ class InverseKinematicsVrepPlugin public: //typedef grl::InverseKinematicsController parent_type; - //using parent_type::currentKinematicsStateP_; //using parent_type::parent_type::InitializeKinematicsState; - + enum VrepVFControllerParamsIndex { DesiredKinematicsObjectName, IKGroupName }; - + typedef std::tuple< std::string // DesiredKinematicsObjectName ,std::string // IKGroupName > Params; - + static Params defaultParams() { return std::make_tuple("RobotMillTipTarget","IK_Group1_iiwa"); } - + /// @todo need to call parent constructor: /*! Constructor */ InverseKinematicsVrepPlugin() { } - + /// @todo TODO(ahundt) accept params for both simulated and measured arms void construct(Params params = defaultParams()){ // get kinematics group name // get number of joints logger_ = spdlog::get("console"); - + // Get the arm that will be used to generate simulated results to command robot // the "base" of this ik is Robotiiwa VrepRobotArmDriverSimulatedP_ = std::make_shared(); @@ -201,10 +246,10 @@ class InverseKinematicsVrepPlugin VrepRobotArmDriverMeasuredP_->construct(); measuredJointHandles_ = VrepRobotArmDriverMeasuredP_->getJointHandles(); measuredJointNames_ = VrepRobotArmDriverMeasuredP_->getJointNames(); - + ikGroupHandle_ = simGetIkGroupHandle(std::get(params).c_str()); simSetExplicitHandling(ikGroupHandle_,1); // enable explicit handling for the ik - + auto armDriverSimulatedParams = VrepRobotArmDriverSimulatedP_->getParams(); // the base frame of the arm ikGroupBaseName_ = (std::get(armDriverSimulatedParams)); @@ -213,27 +258,27 @@ class InverseKinematicsVrepPlugin // the target, or where the tip of the arm should go ikGroupTargetName_ = (std::get(armDriverSimulatedParams)); robotFlangeTipName_ = (std::get(armDriverSimulatedParams)); - - ikGroupBaseHandle_ = grl::vrep::getHandle(ikGroupBaseName_); - ikGroupTipHandle_ = grl::vrep::getHandle(ikGroupTipName_); - ikGroupTargetHandle_ = grl::vrep::getHandle(ikGroupTargetName_); + + ikGroupBaseHandle_ = grl::vrep::getHandle(ikGroupBaseName_); // "Robotiiwa" + ikGroupTipHandle_ = grl::vrep::getHandle(ikGroupTipName_); // "RobotMillTip" + ikGroupTargetHandle_ = grl::vrep::getHandle(ikGroupTargetName_); // "RobotMillTipTarget" /// for why this is named as it is see: https://github.com/jrl-umi3218/Tasks/blob/master/tests/arms.h#L34 sva::PTransform X_base = getObjectPTransform(ikGroupBaseHandle_); //X_base = sva::PTransformd(X_base.rotation().inverse(), X_base.translation()); // start by assuming the base is fixed bool isFixed = true; bool isForwardJoint = true; - + { // Initalize the RBDyn arm representation - + jointHandles_ = VrepRobotArmDriverSimulatedP_->getJointHandles(); jointNames_ = VrepRobotArmDriverSimulatedP_->getJointNames(); - + linkNames_ =VrepRobotArmDriverSimulatedP_->getLinkNames(); linkHandles_ =VrepRobotArmDriverSimulatedP_->getLinkHandles(); linkRespondableNames_ = VrepRobotArmDriverSimulatedP_->getLinkRespondableNames(); linkRespondableHandles_ = VrepRobotArmDriverSimulatedP_->getLinkRespondableHandles(); - + // save the initial joint angles for later // and set the arm to the 0 position for initialization of the // optimizer @@ -244,140 +289,173 @@ class InverseKinematicsVrepPlugin simSetJointPosition(handle,0); initialJointAngles.push_back(angle); } - + rbd_bodyNames_.push_back(ikGroupBaseName_); // note: bodyNames are 1 longer than link names, and start with the base! boost::copy(linkNames_, std::back_inserter(rbd_bodyNames_)); boost::copy(jointNames_, std::back_inserter(rbd_jointNames_)); /// @todo TODO(ahundt) replace hardcoded joint strings with a vrep tree traversal object that generates an RBDyn MultiBodyGraph - jointNames_.push_back("LBR_iiwa_14_R820_link8"); + + // In this part, it added both joints and links into the jointNames_, why? + // Should we add them seperately? + jointNames_.push_back("LBR_iiwa_14_R820_link8"); // Is this a joint? In vrep it's a link. jointNames_.push_back("cutter_joint"); rbd_jointNames_.push_back("LBR_iiwa_14_R820_link8"); rbd_jointNames_.push_back(robotFlangeTipName_); rbd_jointNames_.push_back("cutter_joint"); - rbd_jointNames_.push_back(ikGroupTipName_); - + rbd_jointNames_.push_back(ikGroupTipName_); // "RobotMillTip" + rbd_bodyNames_.push_back("cutter_joint"); rbd_bodyNames_.push_back(ikGroupTipName_); getHandles(rbd_bodyNames_,std::back_inserter(bodyHandles_)); getHandles(rbd_jointNames_,std::back_inserter(rbd_jointHandles_)); - + // Note that V-REP specifies full transforms to place objects that rotate joints around the Z axis - + // based on: https://github.com/jrl-umi3218/Tasks/blob/master/tests/arms.h#L34 // and https://github.com/jrl-umi3218/RBDyn/issues/18#issuecomment-257214536 for(std::size_t i = 0; i < rbd_jointNames_.size(); i++) { - + // Note that V-REP specifies full transforms to place objects that rotate joints around the Z axis std::string thisJointName = rbd_jointNames_[i]; rbd::Joint::Type jointType = rbd::Joint::Fixed; /// @todo TODO(ahundt) fix hard coded Revolute vs fixed joint https://github.com/ahundt/grl/issues/114 - if(simGetObjectType(rbd_jointHandles_[i])==sim_object_joint_type) + if(simGetObjectType(rbd_jointHandles_[i]) == sim_object_joint_type) { jointType = rbd::Joint::Rev; } rbd::Joint j_i(jointType, Eigen::Vector3d::UnitZ(), isForwardJoint, thisJointName); rbd_mbg_.addJoint(j_i); } - + // Note that V-REP specifies full transforms to place objects that rotate joints around the Z axis /// @todo TODO(ahundt) last "joint" RobotMillTip is really fixed... //rbd::Joint j_i(rbd::Joint::Fixed, Eigen::Vector3d::UnitZ(), isForwardJoint, ikGroupTipName_); //rbd::Joint j_i(rbd::Joint::Fixed, Eigen::Vector3d::UnitZ(), isForwardJoint, ikGroupTipName_); //rbd_mbg_.addJoint(j_i); - - + + for(std::size_t i = 0; i < rbd_bodyNames_.size(); i++) { /// @todo TODO(ahundt) extract real inertia and center of mass from V-REP with http://www.coppeliarobotics.com/helpFiles/en/regularApi/simGetShapeMassAndInertia.htm - + double mass = 1.; auto I = Eigen::Matrix3d::Identity(); auto h = Eigen::Vector3d::Zero(); /// @todo TODO(ahundt) consider the origin of the inertia! https://github.com/jrl-umi3218/Tasks/issues/10#issuecomment-257198604 + /// RBInertia is the Spatial Rigid Body Inertia implementation sva::RBInertiad rbi_i(mass, h, I); /// @todo TODO(ahundt) add real support for links, particularly the respondable aspects, see LBR_iiwa_14_R820_joint1_resp in RoboneSimulation.ttt // @todo TODO(ahundt) REMEMBER: The first joint is NOT respondable! - + // remember, bodyNames[0] is the ikGroupBaseName, so entity order is // bodyNames[i], joint[i], bodyNames[i+1] std::string bodyName(rbd_bodyNames_[i]); rbd::Body b_i(rbi_i,bodyName); - + rbd_mbg_.addBody(b_i); } - + // Dummy10 in VRep, by default it's at the origin of the world frame. std::string dummyName0(("Dummy"+ boost::lexical_cast(0+10))); int currentDummy0 = simGetObjectHandle(dummyName0.c_str()); Eigen::Affine3d eto0 = getObjectTransform(rbd_jointHandles_[0],-1); - logger_->info("{} \n{}", dummyName0 , eto0.matrix()); + logger_->info("InverseKinematics: \n {} \n{}", dummyName0 , eto0.matrix()); setObjectTransform(currentDummy0,-1,eto0); - - std::vector frameHandles; + + std::vector frameHandles; // Joint frames frameHandles.push_back(simGetObjectHandle(ikGroupBaseName_.c_str())); boost::copy(rbd_jointHandles_,std::back_inserter(frameHandles)); for(std::size_t i = 0; i < rbd_jointHandles_.size()-1; i++) { // note: Tasks takes transforms in the successor (child link) frame, so it is the inverse of v-rep // thus we are getting the current joint in the frame of the next joint + + // It seems strange that we get the current joint in the frame of the previous one, not the next one? + // to means the transform matrix of joint frameHandles[i+1] relative to frameHandles[i]? + sva::PTransformd to(getObjectPTransform(frameHandles[i+1],frameHandles[i])); // this should be the identity matrix because we set the joints to 0! //sva::PTransformd from(getJointPTransform(rbd_jointHandles_[i+1])); sva::PTransformd from(sva::PTransformd::Identity()); - + // Link the PREVIOUSLY created body to the currently created body with the PREVIOUSLY created joint // remember, bodyNames[0] is the ikGroupBaseName, so entity order is // bodyNames[i], joint[i], bodyNames[i+1] std::string prevBody = rbd_bodyNames_[i]; std::string curJoint = rbd_jointNames_[i]; std::string nextBody = rbd_bodyNames_[i+1]; - + /* prevBody: Body 1 name; + to: Transformation from prevBody to curJoint in prevBody coordinate. + nextBody: Body 2 name. + from: Transformation from body 2 to joint in body 2 coordinate. + curJoint: Joint between the two body name. + isB1toB2 = true: If true use the joint forward value as body 1 to body 2 forward value of the joint and the inverse value for body 2 to body 1 joint. If false the behavior is inversed. + // The body coordinate is fixed at one end of the link, whose origin is coincident with the joint coordinate frame. + // Therefore, the to matrix is the transform matrix between two joints and from matrix is Identity(). + + */ rbd_mbg_.linkBodies(prevBody, to, nextBody, from, curJoint); - } - - + + + // Can't understand this comment, since didn't see the inverse of v-rep? // note: Tasks takes transforms in the successor (child link) frame, so it is the inverse of v-rep + rbd_mbs_.push_back(rbd_mbg_.makeMultiBody(ikGroupBaseName_,isFixed,X_base)); rbd_mbcs_.push_back(rbd::MultiBodyConfig(rbd_mbs_[0])); + // Set the multibody at zero configuration. rbd_mbcs_[0].zero(rbd_mbs_[0]); - + // we only have one robot for the moment so the index of it is 0 const std::size_t simulatedRobotIndex = 0; auto& simArmMultiBody = rbd_mbs_[simulatedRobotIndex]; auto& simArmConfig = rbd_mbcs_[simulatedRobotIndex]; - + + + // update the simulated arm position for (std::size_t i = 0; i < jointHandles_.size(); ++i) { simSetJointPosition(jointHandles_[i],initialJointAngles[i]); } + + // Set RBDyn joint angles from vrep joint angles. SetRBDynArmFromVrep(jointNames_,jointHandles_,simArmMultiBody,simArmConfig); - + rbd::forwardKinematics(simArmMultiBody, simArmConfig); rbd::forwardVelocity(simArmMultiBody, simArmConfig); - + // set the preferred position to the initial position // https://github.com/jrl-umi3218/Tasks/blob/15aff94e3e03f6a161a87799ca2cf262b756bd0c/src/QPTasks.h#L426 rbd_preferred_mbcs_.push_back(simArmConfig); - + + + // std::string homePath = std::getenv("HOME"); + // std::string vrepDataPath = homePath + "/src/V-REP_PRO_EDU_V3_4_0_Linux/data/"; + // std::string urdfPath = vrepDataPath + "Robone_KukaLBRiiwa.urdf"; + + + // auto strRobot = grl::getURDFModel(urdfPath); + // rbd::MultiBody mb = strRobot.mb; + // rbd::MultiBodyConfig mbc(mb); + // rbd::MultiBodyGraph mbg(strRobot.mbg); + // rbd_mbs_.push_back(mb); + // rbd_mbcs_.push_back(mbc); + // // Set the multibody at zero configuration. + // rbd_mbcs_[1].zero(rbd_mbs_[1]); + // std::size_t nrJoints = mb.nrJoints(); + // std::size_t nrBodies = strRobot.mb.nrBodies(); + debugFrames(); - + } - + /// @todo verify object lifetimes - - // add virtual fixtures? need names and number of rows - - + /// add virtual fixtures? need names and number of rows /// @todo read objective rows from vrep - /// @todo set vrep explicit handling of IK here, plus unset in destructor of this object - } - - - + /// Set dummy frames named Dummy0 to Dummy19 to the current state of the /// RBDyn and V-REP representations of joints for debugging. void debugFrames(bool print = false) @@ -389,6 +467,7 @@ class InverseKinematicsVrepPlugin auto& simArmConfig = rbd_mbcs_[simulatedRobotIndex]; // Debug output + // Every dummy represents a joint. for (std::size_t i=0 ; i < jointHandles_.size() ; i++) { Eigen::Affine3d eto; @@ -397,17 +476,18 @@ class InverseKinematicsVrepPlugin eto = getObjectTransform(jointHandles_[i],-1); setObjectTransform(currentDummy2,-1,eto); if(print) logger_->info("{} V-REP World\n{}",dummyName2 , eto.matrix()); - + if(i>0) { Eigen::Affine3d NextJointinPrevFrame(getObjectTransform(jointHandles_[i],jointHandles_[i-1])); if(print) logger_->info("{} V-REP JointInPrevFrame\n{}",dummyName2 , NextJointinPrevFrame.matrix()); } - + bool dummy_world_frame = true; + // The pose of body i is identical with that of joint i? if( dummy_world_frame ) { - // visualize each joint position + // visualize each joint position in world frame sva::PTransform plinkWorld = simArmConfig.bodyPosW[simArmMultiBody.bodyIndexByName(linkNames_[i])]; Eigen::Affine3d linkWorld = PTranformToEigenAffine(plinkWorld); std::string dummyName(("Dummy0"+ boost::lexical_cast(i+1))); @@ -415,14 +495,16 @@ class InverseKinematicsVrepPlugin if(print) logger_->info("{} RBDyn World\n{}",dummyName, linkWorld.matrix()); setObjectTransform(currentDummy,-1,linkWorld); prevDummy=currentDummy; + // Transformation from parent(i) to i in body coordinate (Xj*Xt). sva::PTransform plinkToSon = simArmConfig.parentToSon[simArmMultiBody.bodyIndexByName(linkNames_[i])]; Eigen::Affine3d linkToSon = PTranformToEigenAffine(plinkToSon); if(print) logger_->info("{} RBDyn ParentLinkToSon\n{}",dummyName, linkToSon.matrix()); - + } else { - // visualize each joint position + // visualize each joint position in the previous joint frame + sva::PTransform plinkWorld = simArmConfig.parentToSon[simArmMultiBody.bodyIndexByName(linkNames_[i])]; Eigen::Affine3d linkWorld = PTranformToEigenAffine(plinkWorld); std::string dummyName(("Dummy0"+ boost::lexical_cast(i+1))); @@ -430,48 +512,161 @@ class InverseKinematicsVrepPlugin if(print) logger_->info("{} RBDyn\n{}",dummyName , linkWorld.matrix()); setObjectTransform(currentDummy,prevDummy,linkWorld); prevDummy=currentDummy; - + } } - + // Frame "Dummy" with no numbers goes at the tip! Eigen::Affine3d tipTf = PTranformToEigenAffine(simArmConfig.bodyPosW[simArmMultiBody.bodyIndexByName(ikGroupTipName_)]); setObjectTransform(simGetObjectHandle("Dummy"),-1,tipTf); } + +void ForwardKinematicsFromCSV(int time_index, bool commanddata=false){ + // we only have one robot for the moment so the index of it is 0 + const std::size_t simulatedRobotIndex = 0; + auto& simArmMultiBody = rbd_mbs_[simulatedRobotIndex]; + auto& simArmConfig = rbd_mbcs_[simulatedRobotIndex]; + Eigen::VectorXf joint_angles = Eigen::VectorXf::Zero(7); + std::string homePath = std::getenv("HOME"); + std::string vrepDataPath = homePath + "/src/V-REP_PRO_EDU_V3_4_0_Linux/data/"; + std::string data_inPath = vrepDataPath + "data_in"; + // std::string markerPoseCSV = data_inPath+"/FT_Pose_Marker22.csv"; + std::string FK_CSV = "ForwardKinematics_Pose_M.csv"; + + std::string kukaJoint = data_inPath+"/KUKA_Measured_Joint.csv"; // KUKA_Joint.csv FTKUKA_TimeEvent.csv + if(commanddata) { + kukaJoint = data_inPath+"/KUKA_Command_Joint.csv"; + FK_CSV = "ForwardKinematics_Pose_C.csv"; + } + + + int64_t timeStamp = grl::getJointAnglesFromCSV(kukaJoint, joint_angles, time_index, commanddata); + assert(timeStamp != -1 ); + + std::vector vrepJointAngles; + double angle = 0; + int jointIdx = 0; + std::cout << "Time: " << timeStamp << std::endl; + for(auto i : jointHandles_) + { + angle = joint_angles[jointIdx++]; + simSetJointPosition(i,angle); + vrepJointAngles.push_back(angle); + } + + // Set RBDyn joint angles from vrep joint angles. + SetRBDynArmFromVrep(jointNames_,jointHandles_,simArmMultiBody,simArmConfig); + + rbd::forwardKinematics(simArmMultiBody, simArmConfig); + rbd::forwardVelocity(simArmMultiBody, simArmConfig); + + debugFrames(); + + std::string opticalTrackerDetectedObjectName("Fiducial#22"); + int opticalTrackerDetectedObjectHandle = simGetObjectHandle(opticalTrackerDetectedObjectName.c_str()); + std::string OpticalTrackerBase("OpticalTrackerBase#0"); + int OpticalTrackerBaseHandle = simGetObjectHandle(OpticalTrackerBase.c_str()); + // get fiducial in optical tracker base frame + Eigen::Affine3d transformEstimate = getObjectTransform(opticalTrackerDetectedObjectHandle, OpticalTrackerBaseHandle); + Eigen::VectorXd markerPose = grl::Affine3fToMarkerPose(transformEstimate.cast()); + Eigen::RowVectorXd pose = grl::getPluckerPose(markerPose); + + auto myFile = boost::filesystem::path(vrepDataPath + FK_CSV); + if(time_index == 0 && boost::filesystem::exists(myFile)){ + boost::filesystem::remove(myFile); + std::cout << myFile << " has been removed..." << std::endl; + } + boost::filesystem::ofstream ofs(myFile, std::ios_base::app | std::ios_base::out); + + + if(time_index == 0) { + if(commanddata){ + ofs << "local_request_time_offset_X, Command_KUKA_X, Command_KUKA_Y, Command_KUKA_Z, Command_KUKA_A, Command_KUKA_B, Command_KUKA_C\n"; + }else{ + ofs << "local_receive_time_offset_X, Measured_KUKA_X, Measured_KUKA_Y, Measured_KUKA_Z, Measured_KUKA_A, Measured_KUKA_B, Measured_KUKA_C\n"; + } + } + ofs << timeStamp << ", " << pose[0] << ", " << pose[1] << ", "<< pose[2] << ", "<< pose[3] << ", "<< pose[4] << ", "<< pose[5] << "\n"; + ofs.close(); + + + + + + } + + void replayMarker(std::string filename, int time_index){ + int rowIdx = time_index*10; + //for(int i=rowIdx; i(); + transformEstimate.translation() = eigenPos; + + // set transform between end effector and fiducial + setObjectTransform(markerPoseHandle, OpticalTrackerBaseHandle, transformEstimate); + // logger_->info( "MarkerPose has been set quat wxyz\n: {} , {} , {} , {} \n translation xyz: {} {} {}", + // eigenQuat.w(), eigenQuat.x(), eigenQuat.y(),eigenQuat.z(), transformEstimate.translation().x(), transformEstimate.translation().y(), transformEstimate.translation().z()); + + //} + + } + + void testPose(){ - + /// simulation tick time step in float seconds from vrep API call float simulationTimeStep = simGetSimulationTimeStep(); - + // we only have one robot for the moment so the index of it is 0 const std::size_t simulatedRobotIndex = 0; auto& simArmMultiBody = rbd_mbs_[simulatedRobotIndex]; auto& simArmConfig = rbd_mbcs_[simulatedRobotIndex]; - + + + std::vector jointNames; + std::vector vrepJointAngles; - double angle = 0.7; + double angle = 0; //0.7 for(auto i : jointHandles_) { angle *= -1; simSetJointPosition(i,angle); vrepJointAngles.push_back(angle); } - + + + /// @todo TODO(ahundt) rethink where/when/how to send command for the joint angles. Return to LUA? Set Directly? Distribute via vrep send message command? - + //////////////////////////////////////////////////// // Set joints to current arm position in simulation SetRBDynArmFromVrep(jointNames_,jointHandles_,simArmMultiBody,simArmConfig); rbd::forwardKinematics(simArmMultiBody, simArmConfig); rbd::forwardVelocity(simArmMultiBody, simArmConfig); - + + // SetRBDynArmFromVrep(jointNames_,jointHandles_,rbd_mbs_[1],rbd_mbcs_[1]); + // rbd::forwardKinematics(rbd_mbs_[1],rbd_mbcs_[1]); + // rbd::forwardVelocity(rbd_mbs_[1],rbd_mbcs_[1]); + debugFrames(); } - - + + + + /// Configures updateKinematics with the goal the kinematics should aim for enum class GoalPosE { realGoalPosition, debugGoalPosition }; /// Configures updateKinematics the algorithm the kinematics should use for solving @@ -484,8 +679,13 @@ class InverseKinematicsVrepPlugin /// simulatedArm is the current simulation, measuredArm /// is based off of data measured from the real physical arm enum class ArmStateSource { simulatedArm, measuredArm }; - - + /// Determines which mode to call in run_one function. + /// ik_mode, run the updateKinematics(); + /// replay_mode, replay the collected data from robot, including command and measured joint angles; + /// test_mode, for the calibration purpose in vrep model; + enum class InvRunMode {ik_mode=1, replay_mode=2, test_mode=3 }; + + /// Runs inverse kinematics or constrained optimization at every simulation time step /// @param runOnce Set runOnce = true to only update kinematics once for debugging purposes. runOnce = false runs this function at every time step. /// @param solveForPosition defines where the arm is aiming for, the moving goal pose object or a stationary debug position to verify the algorithm @@ -505,13 +705,12 @@ class InverseKinematicsVrepPlugin ){ if(runOnce && ranOnce_) return; ranOnce_ = true; - + jointHandles_ = VrepRobotArmDriverSimulatedP_->getJointHandles(); - - + /////////////////////////////////////////////////////////// // Copy Joint Interval, the range of motion for each joint - + /// @todo TODO(ahundt) change source of current state based on if physical arm is running if(syncSimulatedKinematicsState == ArmStateSource::simulatedArm) { @@ -522,53 +721,53 @@ class InverseKinematicsVrepPlugin VrepRobotArmDriverMeasuredP_->getState(currentArmState_); } - + // lower limits auto & llim = std::get(currentArmState_); std::vector llimits(llim.begin(),llim.end()); - + // upper limits auto & ulim = std::get(currentArmState_); std::vector ulimits(ulim.begin(),ulim.end()); - + // current position auto & currentJointPos = std::get(currentArmState_); std::vector currentJointPosVec(currentJointPos.begin(),currentJointPos.end()); - + /// @todo TODO(ahundt) also copy acceleration velocity, torque, etc - - + + // Get the current and desired transform in V-Rep const auto& handleParams = VrepRobotArmDriverSimulatedP_->getVrepHandleParams(); Eigen::Affine3d currentEndEffectorPose = getObjectTransform( - std::get(handleParams) + std::get(handleParams) ,std::get(handleParams) ); auto currentEigenT = currentEndEffectorPose.translation(); - - + + Eigen::Affine3d desiredEndEffectorPose = getObjectTransform( - std::get(handleParams) + std::get(handleParams) ,std::get(handleParams) ); auto desiredEigenT = desiredEndEffectorPose.translation(); - + ////////////////////// /// @todo TODO(ahundt) move code below here back into separate independent setup and solve functions, move some steps like limits to construct() - + /// simulation tick time step in float seconds from vrep API call float simulationTimeStep = simGetSimulationTimeStep(); - + // we only have one robot for the moment so the index of it is 0 const std::size_t simulatedRobotIndex = 0; auto& simArmMultiBody = rbd_mbs_[simulatedRobotIndex]; auto& simArmConfig = rbd_mbcs_[simulatedRobotIndex]; - - - + + + /// @todo TODO(ahundt) rethink where/when/how to send command for the joint angles. Return to LUA? Set Directly? Distribute via vrep send message command? - + //////////////////////////////////////////////////// // Set joints to current arm position in simulation if(syncSimulatedKinematicsState == ArmStateSource::simulatedArm) @@ -586,7 +785,7 @@ class InverseKinematicsVrepPlugin } rbd::forwardKinematics(simArmMultiBody, simArmConfig); rbd::forwardVelocity(simArmMultiBody, simArmConfig); - + // save the current MultiBodyConfig for comparison after running algorithms rbd_prev_mbcs_ = rbd_mbcs_; // set the preferred position to the current position @@ -595,7 +794,7 @@ class InverseKinematicsVrepPlugin /// @todo TODO(ahundt) make solver object a member variable if possible, initialize in constructor tasks::qp::QPSolver solver; - + //////////////////////////////////////////////////// // Set position goal of the arm sva::PTransformd targetWorldTransform; @@ -604,42 +803,42 @@ class InverseKinematicsVrepPlugin { // go to the real target position targetWorldTransform = getObjectPTransform(ikGroupTargetHandle_); - logger_->info("target translation (vrep format):\n{}", targetWorldTransform.translation()); + // logger_->info("target translation (vrep format):\n{}", targetWorldTransform.translation()); } else { // go to a debugging target position targetWorldTransform = simArmConfig.bodyPosW[simArmMultiBody.bodyIndexByName(ikGroupTipName_)]; targetWorldTransform.translation().z() -= 0.0001; - logger_->info("target translation (rbdyn format):\n", targetWorldTransform.translation()); + // logger_->info("target translation (rbdyn format):\n", targetWorldTransform.translation()); } tasks::qp::PositionTask posTask(rbd_mbs_, simulatedRobotIndex, ikGroupTipName_,targetWorldTransform.translation()); tasks::qp::SetPointTask posTaskSp(rbd_mbs_, simulatedRobotIndex, &posTask, 50., 10.); tasks::qp::OrientationTask oriTask(rbd_mbs_,simulatedRobotIndex, ikGroupTipName_,targetWorldTransform.rotation()); tasks::qp::SetPointTask oriTaskSp(rbd_mbs_, simulatedRobotIndex, &oriTask, 50., 1.); tasks::qp::PostureTask postureTask(rbd_mbs_,simulatedRobotIndex,rbd_preferred_mbcs_[simulatedRobotIndex].q,1,0.01); - + double inf = std::numeric_limits::infinity(); - - + + //////////////////////////////////// // Set joint limits - + // joint limit objects, initialize to current q so entries // will be reasonable, such as empty entries for fixed joints std::vector > lBound = simArmConfig.q; std::vector > uBound = simArmConfig.q; std::vector > lVelBound = simArmConfig.alpha; std::vector > uVelBound = simArmConfig.alpha; - - + + // for all joints for (std::size_t i=0 ; i < rbd_jointHandles_.size()-1; i++) { /// limits must be organized as described in https://github.com/jrl-umi3218/Tasks/issues/10#issuecomment-257793242 std::string jointName = rbd_jointNames_[i]; std::size_t jointIdx = simArmMultiBody.jointIndexByName(jointName); - + /// @todo TODO(ahundt) ulimits aren't the same size as jointHandles_, need velocity limits too // set joint position limits if(boost::iequals(jointName,"cutter_joint")) @@ -654,7 +853,7 @@ class InverseKinematicsVrepPlugin lBound[jointIdx][0] = llimits[i]; uBound[jointIdx][0] = ulimits[i]; } - + // set joint velocity limits if(iinfo("run ik_mode...."); + break; + case InvRunMode::replay_mode: + ForwardKinematicsFromCSV(time_index, commanddata); + // replayMarker(markerPose, time_index); + time_index++; + // logger_->info("run replay_mode.... and commanddata {}", commanddata); + break; + case InvRunMode::test_mode: + testPose(); + // logger_->info("run test_mode...."); + break; + default: + logger_->info("Wrong parameter for run_mode, the only options are ik_mode=0, replay_mode=1, and test_mode=2 "); + } + } + /// Call this function in construct function to determine the running mode and running data. + void setInvRunMode(int _run_mode = 1, bool _commanddata = false) { + switch(_run_mode){ + case 1: + run_mode = InvRunMode::ik_mode; + break; + case 2: + run_mode = InvRunMode::replay_mode; + break; + case 3: + run_mode = InvRunMode::test_mode; + break; + default: + run_mode = InvRunMode::ik_mode; + std::cout << "_run_mode: " << _run_mode << "is beyond index..." << std::endl; + } + commanddata = _commanddata; + std::cout << "_run_mode: " << _run_mode << " commanddata" << commanddata << std::endl; } - - /// may not need this it is in the base class /// this will have output /// blocking call, call in separate thread, just allocates memory - /// this runs the actual optimization algorithm + /// this runs the actual optimization algorithmn void solve(){ - + } - + /// for the replay purpose. + int time_index = 0; + fbs_tk::Root kukaStatesP_; + std::shared_ptr logger_; rbd::MultiBodyGraph rbd_mbg_; @@ -785,35 +1022,39 @@ class InverseKinematicsVrepPlugin /// because we need additional fixed "joints" so we have transforms /// that match the V-REP scene std::vector rbd_jointHandles_; - - + + //tasks::qp::QPSolver qp_solver_; - + std::vector jointHandles_; ///< @todo move this item back into VrepRobotArmDriver std::vector linkHandles_; std::vector linkRespondableHandles_; std::vector jointNames_; std::vector linkNames_; std::vector linkRespondableNames_; - - + + std::vector measuredJointHandles_; ///< @todo move this item back into VrepRobotArmDriver std::vector measuredJointNames_; - + int ikGroupHandle_; int ikGroupBaseHandle_; int ikGroupTipHandle_; int ikGroupTargetHandle_; - + std::string ikGroupBaseName_; std::string ikGroupTipName_; std::string ikGroupTargetName_; std::string robotFlangeTipName_; // not part of the V-REP ik group - + std::shared_ptr VrepRobotArmDriverSimulatedP_; std::shared_ptr VrepRobotArmDriverMeasuredP_; vrep::VrepRobotArmDriver::State currentArmState_; + /// These two parameters are for the replay process. + InvRunMode run_mode = InvRunMode::ik_mode; + bool commanddata = false; // Determines what kind of joint angles to run, command or measured? + bool ranOnce_ = false; EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; diff --git a/include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp b/include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp index 5f5184a..84c9267 100644 --- a/include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp +++ b/include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp @@ -16,15 +16,12 @@ namespace grl { namespace vrep { - - - /// Creates a complete vrep plugin object /// usage: /// @code -/// auto kukaPluginPG = std::make_shared(); -/// kukaPluginPG->construct(); -/// while(true) kukaPluginPG->run_one(); +/// auto kukaVrepPluginPG = std::make_shared(); +/// kukaVrepPluginPG->construct(); +/// while(true) kukaVrepPluginPG->run_one(); /// @endcode /// /// @todo this implementation is a bit hacky, redesign it @@ -52,7 +49,7 @@ class KukaVrepPlugin : public std::enable_shared_from_this { KukaMonitorMode, IKGroupName }; - + typedef std::tuple< std::vector, std::string, @@ -71,11 +68,10 @@ class KukaVrepPlugin : public std::enable_shared_from_this { std::string, std::string > Params; - - + static const Params defaultParams(){ std::vector jointHandles; - + jointHandles.push_back("LBR_iiwa_14_R820_joint1"); // Joint1Handle, jointHandles.push_back("LBR_iiwa_14_R820_joint2"); // Joint2Handle, jointHandles.push_back("LBR_iiwa_14_R820_joint3"); // Joint3Handle, @@ -102,12 +98,12 @@ class KukaVrepPlugin : public std::enable_shared_from_this { "IK_Group1_iiwa" // IKGroupName ); } - + /// @todo measuredArmParams are hardcoded, parameterize them - // parameters for measured arm + /// parameters for measured arm static const Params measuredArmParams(){ std::vector jointHandles; - + jointHandles.push_back("LBR_iiwa_14_R820_joint1#0"); // Joint1Handle, jointHandles.push_back("LBR_iiwa_14_R820_joint2#0"); // Joint2Handle, jointHandles.push_back("LBR_iiwa_14_R820_joint3#0"); // Joint3Handle, @@ -135,316 +131,334 @@ class KukaVrepPlugin : public std::enable_shared_from_this { ); } + /// @todo allow KukaFRIThreadSeparator parameters to be updated + /// @param params a tuple containing all of the parameter strings needed to configure the device. + /// + /// The KukaCommandMode parameters supports the options "FRI" and "JAVA". This configures how commands will + /// be sent to the arm itself. "FRI" mode is via a direct "Fast Robot Interface" "KUKA KONI" + /// ethernet connection which provides substantially higher performance and response time, + /// but is extremely sensitive to delays, and any delay will halt the robot and require a manual reset. + /// "JAVA" mode sends the command to the Java application installed on the KUKA robot, which then submits + /// it to the arm itself to execute. This is a much more forgiving mode of communication, but it is subject to delays. + /// + /// @todo read ms_per_tick from the java interface + KukaVrepPlugin (Params params = defaultParams()) : params_(params) + { + logger_ = spdlog::get("console"); + } + void construct(){construct(params_);} + + /// construct() function completes initialization of the plugin + /// @todo move this into the actual constructor, but need to correctly handle or attach vrep shared libraries for unit tests. + void construct(Params params){ + params_ = params; + // keep driver threads from exiting immediately after creation, because they have work to do! + device_driver_workP_.reset(new boost::asio::io_service::work(device_driver_io_service)); + kukaDriverP_=std::make_shared(std::make_tuple( + std::get(params), + std::get(params), + std::get(params), + std::get(params), + std::get(params), + std::get(params), + std::get(params), + std::get(params), + std::get(params), + std::get(params), + std::get(params) + )); + kukaDriverP_->construct(); + // Default to joint servo mode for commanding motion + kukaDriverP_->set(flatbuffer::ArmState::MoveArmJointServo); + // std::cout << "KUKA COMMAND MODE: " << std::get(params) << "\n"; + initHandles(); + } + /// start recording the kuka state data in memory + /// return true on success, false on failure + bool start_recording(int single_buffer_limit_bytes) + { + if(kukaDriverP_.get() != nullptr) { + return kukaDriverP_->start_recording(single_buffer_limit_bytes); + } + std::cout << "kukaDriverP_ is nullptr..." << single_buffer_limit_bytes << std::endl; + return false; + } + /// stop recording the kuka state data in memory + /// return true on success, false on failure + bool stop_recording() + { + if(kukaDriverP_.get() != nullptr) { + return kukaDriverP_->stop_recording(); + } + return false; + } + bool save_recording(std::string filename = std::string()) { + if(kukaDriverP_.get() != nullptr) { + return kukaDriverP_->save_recording(filename); + } + return false; + } + void clear_recording() + { + if(kukaDriverP_.get() != nullptr) { + kukaDriverP_->clear_recording(); + } + } + bool is_recording() + { if(kukaDriverP_.get() != nullptr) { + // std::cout<<"is_recording: " <is_recording() <is_recording(); + } + return false; + } -/// @todo allow KukaFRIThreadSeparator parameters to be updated -/// @param params a tuple containing all of the parameter strings needed to configure the device. -/// -/// The KukaCommandMode parameters supports the options "FRI" and "JAVA". This configures how commands will -/// be sent to the arm itself. "FRI" mode is via a direct "Fast Robot Interface" "KUKA KONI" -/// ethernet connection which provides substantially higher performance and response time, -/// but is extremely sensitive to delays, and any delay will halt the robot and require a manual reset. -/// "JAVA" mode sends the command to the Java application installed on the KUKA robot, which then submits -/// it to the arm itself to execute. This is a much more forgiving mode of communication, but it is subject to delays. -/// -/// @todo read ms_per_tick from the java interface -KukaVrepPlugin (Params params = defaultParams()) - : - params_(params) -{ - logger_ = spdlog::get("console"); -} - -void construct(){construct(params_);} - -/// construct() function completes initialization of the plugin -/// @todo move this into the actual constructor, but need to correctly handle or attach vrep shared libraries for unit tests. -void construct(Params params){ - params_ = params; - // keep driver threads from exiting immediately after creation, because they have work to do! - device_driver_workP_.reset(new boost::asio::io_service::work(device_driver_io_service)); - kukaDriverP_=std::make_shared(std::make_tuple( - - std::get(params), - std::get(params), - std::get(params), - std::get(params), - std::get(params), - std::get(params), - std::get(params), - std::get(params), - std::get(params), - std::get(params), - std::get(params) - - - )); - kukaDriverP_->construct(); - // Default to joint servo mode for commanding motion - kukaDriverP_->set(flatbuffer::ArmState::MoveArmJointServo); - std::cout << "KUKA COMMAND MODE: " << std::get(params) << "\n"; - initHandles(); -} - - -void run_one(){ - - if(!allHandlesSet) BOOST_THROW_EXCEPTION(std::runtime_error("KukaVrepPlugin: Handles have not been initialized, cannot run updates.")); - getRealKukaAngles(); - bool isError = getStateFromVrep(); // true if there is an error - allHandlesSet = !isError; - /// @todo re-enable simulation feedback based on actual kuka state - syncVrepAndKuka(); -} - - -~KukaVrepPlugin(){ - device_driver_workP_.reset(); - - if(driver_threadP){ - device_driver_io_service.stop(); - driver_threadP->join(); + void run_one(){ + + if(!allHandlesSet) BOOST_THROW_EXCEPTION(std::runtime_error("KukaVrepPlugin: Handles have not been initialized, cannot run updates.")); + getRealKukaAngles(); + bool isError = getStateFromVrep(); // true if there is an error + allHandlesSet = !isError; + /// @todo re-enable simulation feedback based on actual kuka state + syncVrepAndKuka(); + } + + + ~KukaVrepPlugin(){ + device_driver_workP_.reset(); + + if(driver_threadP){ + device_driver_io_service.stop(); + driver_threadP->join(); + } + } + + private: + + void initHandles() { + + vrepRobotArmDriverP_ = std::make_shared + ( + std::make_tuple( + std::get (params_), + std::get (params_), + std::get (params_), + std::get (params_), + std::get (params_), + std::get (params_) + ) + ); + + vrepRobotArmDriverP_->construct(); + measuredParams_ = measuredArmParams(); + vrepMeasuredRobotArmDriverP_ = std::make_shared + ( + std::make_tuple( + std::get (measuredParams_), + std::get (measuredParams_), + std::get (measuredParams_), + std::get (measuredParams_), + std::get (measuredParams_), + std::get (measuredParams_) + ) + ); + vrepMeasuredRobotArmDriverP_->construct(); + /// @todo remove this assumption + allHandlesSet = true; + } + + void getRealKukaAngles() { + /// @todo TODO(ahundt) m_haveReceivedRealData = true is a DANGEROUS HACK!!!! REMOVE ME AFTER DEBUGGING + m_haveReceivedRealData = true; } -} - -private: - - - -void initHandles() { - - vrepRobotArmDriverP_ = std::make_shared - ( - std::make_tuple( - std::get (params_), - std::get (params_), - std::get (params_), - std::get (params_), - std::get (params_), - std::get (params_) - ) - ); - - - vrepRobotArmDriverP_->construct(); - - measuredParams_ = measuredArmParams(); - vrepMeasuredRobotArmDriverP_ = std::make_shared - ( - std::make_tuple( - std::get (measuredParams_), - std::get (measuredParams_), - std::get (measuredParams_), - std::get (measuredParams_), - std::get (measuredParams_), - std::get (measuredParams_) - ) - ); - vrepMeasuredRobotArmDriverP_->construct(); - - - /// @todo remove this assumption - allHandlesSet = true; -} - -void getRealKukaAngles() { - /// @todo TODO(ahundt) m_haveReceivedRealData = true is a DANGEROUS HACK!!!! REMOVE ME AFTER DEBUGGING - m_haveReceivedRealData = true; - - - -} - -void syncVrepAndKuka(){ - + + void syncVrepAndKuka(){ + bool debug = false; + if(!allHandlesSet || !m_haveReceivedRealData) return; - /// @todo make this handled by template driver implementations/extensions - kukaDriverP_->set(simulationTimeStep_,time_duration_command_tag()); + kukaDriverP_->set(simulationTimeStep_, time_duration_command_tag()); kukaDriverP_->set( simJointPosition, grl::revolute_joint_angle_open_chain_command_tag()); - if(0) kukaDriverP_->set( simJointForce , grl::revolute_joint_torque_open_chain_command_tag()); - - + if(debug) kukaDriverP_->set( simJointForce, grl::revolute_joint_torque_open_chain_command_tag()); kukaDriverP_->run_one(); // We have the real kuka state read from the device now // update real joint angle data realJointPosition.clear(); kukaDriverP_->get(std::back_inserter(realJointPosition), grl::revolute_joint_angle_open_chain_state_tag()); - realJointForce.clear(); kukaDriverP_->get(std::back_inserter(realJointForce), grl::revolute_joint_torque_open_chain_state_tag()); - realExternalJointTorque.clear(); kukaDriverP_->get(std::back_inserter(realExternalJointTorque), grl::revolute_joint_torque_external_open_chain_state_tag()); - realExternalForce.clear(); kukaDriverP_->get(std::back_inserter(realExternalForce), grl::cartesian_external_force_tag()); - - if(0){ - // debug output - std::cout << "Measured Torque: "; - std::cout << std::setw(6); - for (float t:realJointForce) { - std::cout << t << " "; - } - std::cout << '\n'; - - std::cout << "External Torque: "; - std::cout << std::setw(6); - for (float t:realExternalJointTorque) { - std::cout << t << " "; + + if(debug){ + // debug output + std::cout << "Measured Torque: "; + std::cout << std::setw(6); + for (float t:realJointForce) { + std::cout << t << " "; + } + std::cout << '\n'; + + std::cout << "External Torque: "; + std::cout << std::setw(6); + for (float t:realExternalJointTorque) { + std::cout << t << " "; + } + std::cout << '\n'; + + std::cout << "Measured Position: "; + for (float t:realJointPosition) { + std::cout << t << " "; + } + std::cout << '\n'; } - std::cout << '\n'; - - std::cout << "Measured Position: "; - for (float t:realJointPosition) { - std::cout << t << " "; + + if(!allHandlesSet || !vrepMeasuredRobotArmDriverP_) return; + + if (realJointPosition.size() > 0) { // if there are valid measured states + VrepRobotArmDriver::State measuredArmState; + std::get(measuredArmState) = realJointPosition; + std::get(measuredArmState) = realJointForce; + std::get(measuredArmState) = realExternalJointTorque; + + vrepMeasuredRobotArmDriverP_->setState(measuredArmState); } - std::cout << '\n'; } - - if(!allHandlesSet || !vrepMeasuredRobotArmDriverP_) return; - - if (realJointPosition.size() > 0) { // if there are valid measured states - VrepRobotArmDriver::State measuredArmState; - std::get(measuredArmState) = realJointPosition; - std::get(measuredArmState) = realJointForce; - std::get(measuredArmState) = realExternalJointTorque; - - vrepMeasuredRobotArmDriverP_->setState(measuredArmState); + + /// @todo if there aren't real limits set via the kuka model already then implement me + void setArmLimits(){ + //simSetJointInterval(simInt objectHandle,simBool cyclic,const simFloat* interval); //Sets the interval parameters of a joint (i.e. range values) + //simSetJointMode(simInt jointHandle,simInt jointMode,simInt options); //Sets the operation mode of a joint. Might have as side-effect the change of additional properties of the joint + + } + + /// @return isError - true if there is an error, false if there is no Error + bool getStateFromVrep(){ + if(!allHandlesSet || !vrepRobotArmDriverP_) return false; + + VrepRobotArmDriver::State armState; + + bool isError = vrepRobotArmDriverP_->getState(armState); + + if(isError) return isError; + + simJointPosition = std::get (armState); + simJointForce = std::get (armState); + simJointTargetPosition = std::get(armState); + simJointTransformationMatrix = std::get (armState); + + + /// need to provide tick time in double seconds and get from vrep API call + simulationTimeStep_ = simGetSimulationTimeStep()*1000; + + // for (int i=0 ; i < KUKA::LBRState::NUM_DOF ; i++) + // { + // simGetJointPosition(jointHandle[i],&simJointPosition[i]); //retrieves the intrinsic position of a joint (Angle for revolute joint) + // simGetJointForce(jointHandle[i],&simJointForce[i]); //retrieves the force or torque applied to a joint along/about its active axis. This function retrieves meaningful information only if the joint is prismatic or revolute, and is dynamically enabled. + // simGetJointTargetPosition(jointHandle[i],&simJointTargetPosition[i]); //retrieves the target position of a joint + // simGetJointMatrix(jointHandle[i],&simJointTransformationMatrix[i]); //retrieves the intrinsic transformation matrix of a joint (the transformation caused by the joint movement) + // + // } + // + // BOOST_LOG_TRIVIAL(info) << "simJointPostition = " << simJointPosition << std::endl; + // BOOST_LOG_TRIVIAL(info) << "simJointForce = " << simJointForce << std::endl; + // BOOST_LOG_TRIVIAL(info) << "simJointTargetPostition = " << simJointTargetPosition << std::endl; + // BOOST_LOG_TRIVIAL(info) << "simJointTransformationMatrix = " << simJointTransformationMatrix << std::endl; + // + // float simTipPosition[3]; + // float simTipOrientation[3]; + // + // simGetObjectPosition(target, targetBase, simTipPosition); + // simGetObjectOrientation(target, targetBase, simTipOrientation); + // + // for (int i = 0 ; i < 3 ; i++) + // { + // BOOST_LOG_TRIVIAL(info) << "simTipPosition[" << i << "] = " << simTipPosition[i] << std::endl; + // BOOST_LOG_TRIVIAL(info) << "simTipOrientation[" << i << "] = " << simTipOrientation[i] << std::endl; + // + // } + // Send updated position to the real arm based on simulation + return false; } - -} - -/// @todo if there aren't real limits set via the kuka model already then implement me -void setArmLimits(){ - //simSetJointInterval(simInt objectHandle,simBool cyclic,const simFloat* interval); //Sets the interval parameters of a joint (i.e. range values) - //simSetJointMode(simInt jointHandle,simInt jointMode,simInt options); //Sets the operation mode of a joint. Might have as side-effect the change of additional properties of the joint - -} - -/// @return isError - true if there is an error, false if there is no Error -bool getStateFromVrep(){ - if(!allHandlesSet || !vrepRobotArmDriverP_) return false; - - VrepRobotArmDriver::State armState; - - bool isError = vrepRobotArmDriverP_->getState(armState); - - if(isError) return isError; - - simJointPosition = std::get (armState); - simJointForce = std::get (armState); - simJointTargetPosition = std::get(armState); - simJointTransformationMatrix = std::get (armState); - - - /// need to provide tick time in double seconds and get from vrep API call - simulationTimeStep_ = simGetSimulationTimeStep()*1000; - -// for (int i=0 ; i < KUKA::LBRState::NUM_DOF ; i++) -// { -// simGetJointPosition(jointHandle[i],&simJointPosition[i]); //retrieves the intrinsic position of a joint (Angle for revolute joint) -// simGetJointForce(jointHandle[i],&simJointForce[i]); //retrieves the force or torque applied to a joint along/about its active axis. This function retrieves meaningful information only if the joint is prismatic or revolute, and is dynamically enabled. -// simGetJointTargetPosition(jointHandle[i],&simJointTargetPosition[i]); //retrieves the target position of a joint -// simGetJointMatrix(jointHandle[i],&simJointTransformationMatrix[i]); //retrieves the intrinsic transformation matrix of a joint (the transformation caused by the joint movement) -// -// } -// -// BOOST_LOG_TRIVIAL(info) << "simJointPostition = " << simJointPosition << std::endl; -// BOOST_LOG_TRIVIAL(info) << "simJointForce = " << simJointForce << std::endl; -// BOOST_LOG_TRIVIAL(info) << "simJointTargetPostition = " << simJointTargetPosition << std::endl; -// BOOST_LOG_TRIVIAL(info) << "simJointTransformationMatrix = " << simJointTransformationMatrix << std::endl; -// -// float simTipPosition[3]; -// float simTipOrientation[3]; -// -// simGetObjectPosition(target, targetBase, simTipPosition); -// simGetObjectOrientation(target, targetBase, simTipOrientation); -// -// for (int i = 0 ; i < 3 ; i++) -// { -// BOOST_LOG_TRIVIAL(info) << "simTipPosition[" << i << "] = " << simTipPosition[i] << std::endl; -// BOOST_LOG_TRIVIAL(info) << "simTipOrientation[" << i << "] = " << simTipOrientation[i] << std::endl; -// -// } - // Send updated position to the real arm based on simulation - return false; -} - -/// @todo reenable this component -bool updateVrepFromKuka() { - // Step 1 - //////////////////////////////////////////////////// - // call the functions here and just print joint angles out - // or display something on the screens - - /////////////////// - // call our object to get the latest real kuka state - // then use the functions below to set the simulation state - // to match - - /////////////////// assuming given real joint position (angles), forces, target position and target velocity - - - // setting the simulation variables to data from real robot (here they have been assumed given) - - for (int i=0 ; i < 7 ; i++) - { - //simSetJointPosition(jointHandle[i],realJointPosition[i]); //Sets the intrinsic position of a joint. May have no effect depending on the joint mode - - - //simSetJointTargetPosition(jointHandle[i],realJointTargetPosition[i]); //Sets the target position of a joint if the joint is in torque/force mode (also make sure that the joint's motor and position control are enabled - - //simSetJointForce(jointHandle[i],realJointForce[i]); //Sets the maximum force or torque that a joint can exert. This function has no effect when the joint is not dynamically enabled - - - //simSetJointTargetVelocity(jointHandle[i],realJointTargetVelocity[i]); //Sets the intrinsic target velocity of a non-spherical joint. This command makes only sense when the joint mode is: (a) motion mode: the joint's motion handling feature must be enabled (simHandleJoint must be called (is called by default in the main script), and the joint motion properties must be set in the joint settings dialog), (b) torque/force mode: the dynamics functionality and the joint motor have to be enabled (position control should however be disabled) - } - return false; -} - -volatile bool allHandlesSet = false; -volatile bool m_haveReceivedRealData = false; - -double simulationTimeStep_; // ms - -boost::asio::io_service device_driver_io_service; -std::unique_ptr device_driver_workP_; -std::unique_ptr driver_threadP; -std::shared_ptr kukaDriverP_; -std::shared_ptr vrepRobotArmDriverP_; -std::shared_ptr vrepMeasuredRobotArmDriverP_; -//boost::asio::deadline_timer sendToJavaDelay; - -/// @todo replace all these simJoint elements with simple VrepRobotArmDriver::State -std::vector simJointPosition = {0.0,0.0,0.0,0.0,0.0,0.0,0.0}; -// std::vector simJointVelocity = {0.0,0.0,0.0,0.0,0.0,0.0,0.0}; no velocity yet -std::vector simJointForce = {0.0,0.0,0.0,0.0,0.0,0.0,0.0}; -std::vector simJointTargetPosition = {0.0,0.0,0.0,0.0,0.0,0.0,0.0}; -VrepRobotArmDriver::TransformationMatrices simJointTransformationMatrix; - -/// @note loss of precision! kuka sends double values, if you write custom code don't use these float values. Vrep uses floats internally which is why they are used here. -std::vector realJointPosition = { 0, 0, 0, 0, 0, 0, 0 }; -// does not exist -// std::vector realJointTargetPosition = { 0, 0, 0, 0, 0, 0, 0 }; -std::vector realJointForce = { 0, 0, 0, 0, 0, 0, 0 }; -// does not exist yet -//std::vector realJointTargetVelocity = { 0, 0, 0, 0, 0, 0, 0 }; -std::vector realExternalJointTorque = { 0, 0, 0, 0, 0, 0, 0}; -std::vector realExternalForce = { 0, 0, 0, 0, 0, 0}; -private: -Params params_; -Params measuredParams_; -/// for use in FRI clientdata mode -std::shared_ptr friData_; -std::shared_ptr logger_; + + /// @todo reenable this component + bool updateVrepFromKuka() { + // Step 1 + //////////////////////////////////////////////////// + // call the functions here and just print joint angles out + // or display something on the screens + + /////////////////// + // call our object to get the latest real kuka state + // then use the functions below to set the simulation state + // to match + + /////////////////// assuming given real joint position (angles), forces, target position and target velocity + + + // setting the simulation variables to data from real robot (here they have been assumed given) + + for (int i=0 ; i < 7 ; i++) + { + //simSetJointPosition(jointHandle[i],realJointPosition[i]); //Sets the intrinsic position of a joint. May have no effect depending on the joint mode + + + //simSetJointTargetPosition(jointHandle[i],realJointTargetPosition[i]); //Sets the target position of a joint if the joint is in torque/force mode (also make sure that the joint's motor and position control are enabled + + //simSetJointForce(jointHandle[i],realJointForce[i]); //Sets the maximum force or torque that a joint can exert. This function has no effect when the joint is not dynamically enabled + + + //simSetJointTargetVelocity(jointHandle[i],realJointTargetVelocity[i]); //Sets the intrinsic target velocity of a non-spherical joint. This command makes only sense when the joint mode is: (a) motion mode: the joint's motion handling feature must be enabled (simHandleJoint must be called (is called by default in the main script), and the joint motion properties must be set in the joint settings dialog), (b) torque/force mode: the dynamics functionality and the joint motor have to be enabled (position control should however be disabled) + } + return false; + } + + volatile bool allHandlesSet = false; + volatile bool m_haveReceivedRealData = false; + + double simulationTimeStep_; // ms + + boost::asio::io_service device_driver_io_service; + // CThe work class is used to inform the io_service when work starts and finishes. + // This ensures that the io_service object's run() function will not exit while work is underway, + // and that it does exit when there is no unfinished work remaining. + std::unique_ptr device_driver_workP_; + std::unique_ptr driver_threadP; + std::shared_ptr kukaDriverP_; + std::shared_ptr vrepRobotArmDriverP_; + std::shared_ptr vrepMeasuredRobotArmDriverP_; + //boost::asio::deadline_timer sendToJavaDelay; + + /// @todo replace all these simJoint elements with simple VrepRobotArmDriver::State + std::vector simJointPosition = {0.0,0.0,0.0,0.0,0.0,0.0,0.0}; + // std::vector simJointVelocity = {0.0,0.0,0.0,0.0,0.0,0.0,0.0}; no velocity yet + std::vector simJointForce = {0.0,0.0,0.0,0.0,0.0,0.0,0.0}; + std::vector simJointTargetPosition = {0.0,0.0,0.0,0.0,0.0,0.0,0.0}; + VrepRobotArmDriver::TransformationMatrices simJointTransformationMatrix; + + /// @note loss of precision! kuka sends double values, if you write custom code don't use these float values. Vrep uses floats internally which is why they are used here. + std::vector realJointPosition = { 0, 0, 0, 0, 0, 0, 0 }; + // does not exist + // std::vector realJointTargetPosition = { 0, 0, 0, 0, 0, 0, 0 }; + std::vector realJointForce = { 0, 0, 0, 0, 0, 0, 0 }; + // does not exist yet + //std::vector realJointTargetVelocity = { 0, 0, 0, 0, 0, 0, 0 }; + std::vector realExternalJointTorque = { 0, 0, 0, 0, 0, 0, 0}; + std::vector realExternalForce = { 0, 0, 0, 0, 0, 0}; + private: + Params params_; + Params measuredParams_; + /// for use in FRI clientdata mode + std::shared_ptr friData_; + std::shared_ptr logger_; }; - + }} // grl::vrep #endif diff --git a/include/grl/vrep/Vrep.hpp b/include/grl/vrep/Vrep.hpp index 048e620..e0da032 100644 --- a/include/grl/vrep/Vrep.hpp +++ b/include/grl/vrep/Vrep.hpp @@ -2,10 +2,12 @@ #ifndef GRL_VREP_HPP_ #define GRL_VREP_HPP_ +#include #include #include #include "v_repLib.h" + namespace grl { namespace vrep { /// @brief Get the handle of a single object, otherwise throw an exception diff --git a/include/grl/vrep/VrepRobotArmDriver.hpp b/include/grl/vrep/VrepRobotArmDriver.hpp index b360ba6..bb269a8 100644 --- a/include/grl/vrep/VrepRobotArmDriver.hpp +++ b/include/grl/vrep/VrepRobotArmDriver.hpp @@ -39,19 +39,19 @@ namespace grl { namespace vrep { linkNames.push_back("LBR_iiwa_14_R820_link8"); return linkNames; } - + /// @todo TODO(ahundt) HACK joint to link and joint to respondable only works for v-rep provided scene heirarchy names, modify full class so these workarounds aren't needed. /// @see jointToLink std::vector jointToLinkRespondable(std::vector jointNames) { auto linkNames = jointToLink(jointNames); - + std::vector linkNames_resp; - + int i = 1; for(std::string linkName : linkNames) { - /// @todo TODO(ahundt) link 1 isn't respondable because it is anchored to the ground, but should there be an empty string so the indexes are consistent or skip it entirely? (currently skipping) + /// @todo TODO(ahundt) link 1 isn't respondable because it is anchored to the ground, but should there be an empty string so the indexes are consistent or skip it entirely? (currently skipping) if(i!=1) { boost::algorithm::replace_last(linkName,"link" + boost::lexical_cast(i),"link" + boost::lexical_cast(i) + "_resp"); @@ -59,14 +59,14 @@ namespace grl { namespace vrep { } ++i; } - + return linkNames_resp; } /// @brief C++ interface for any open chain V-REP robot arm /// -/// VrepRobotArmDriver makes it easy to specify and interact with the -/// joints in a V-REP defined robot arm in a consistent manner. +/// VrepRobotArmDriver makes it easy to specify and interact with the +/// joints in a V-REP defined robot arm in a consistent manner. /// /// @todo add support for links, particularly the respondable aspects, see LBR_iiwa_14_R820_joint1_resp in RoboneSimulation.ttt /// @todo write generic getters and setters for this object like in KukaFRIalgorithm.hpp and the member functions of KukaFRIdriver, KukaJAVAdriver @@ -81,7 +81,7 @@ class VrepRobotArmDriver : public std::enable_shared_from_this, std::string, @@ -90,7 +90,7 @@ class VrepRobotArmDriver : public std::enable_shared_from_this Params; - + typedef std::tuple< std::vector, int, @@ -99,19 +99,19 @@ class VrepRobotArmDriver : public std::enable_shared_from_this VrepHandleParams; - + static const Params defaultParams() { std::vector jointNames{ "LBR_iiwa_14_R820_joint1" , // Joint1Handle, - "LBR_iiwa_14_R820_joint2" , // Joint2Handle, - "LBR_iiwa_14_R820_joint3" , // Joint3Handle, - "LBR_iiwa_14_R820_joint4" , // Joint4Handle, - "LBR_iiwa_14_R820_joint5" , // Joint5Handle, - "LBR_iiwa_14_R820_joint6" , // Joint6Handle, + "LBR_iiwa_14_R820_joint2" , // Joint2Handle, + "LBR_iiwa_14_R820_joint3" , // Joint3Handle, + "LBR_iiwa_14_R820_joint4" , // Joint4Handle, + "LBR_iiwa_14_R820_joint5" , // Joint5Handle, + "LBR_iiwa_14_R820_joint6" , // Joint6Handle, "LBR_iiwa_14_R820_joint7" // Joint7Handle, }; - + return std::make_tuple( jointNames , // JointNames "RobotFlangeTip" , // RobotFlangeTipName, @@ -121,7 +121,7 @@ class VrepRobotArmDriver : public std::enable_shared_from_this jointNames{ @@ -133,7 +133,7 @@ class VrepRobotArmDriver : public std::enable_shared_from_this JointScalar; - + /// @see http://www.coppeliarobotics.com/helpFiles/en/apiFunctions.htm#simGetJointMatrix for data layout information typedef std::array TransformationMatrix; typedef std::vector TransformationMatrices; - + typedef std::tuple< JointScalar, // jointPosition // JointScalar // JointVelocity // no velocity yet @@ -178,19 +176,19 @@ class VrepRobotArmDriver : public std::enable_shared_from_this State; - - + + VrepRobotArmDriver(Params params = defaultParams()) : params_(params) { } - + /// @todo create a function that calls simGetObjectHandle and throws an exception when it fails /// @warning getting the ik group is optional, so it does not throw an exception void construct() { std::vector jointHandle; getHandleFromParam(params_,std::back_inserter(jointHandle)); - handleParams_ = + handleParams_ = std::make_tuple( std::move(jointHandle) //Obtain Joint Handles ,getHandleFromParam (params_) //Obtain RobotTip handle @@ -199,7 +197,7 @@ void construct() { ,getHandleFromParam (params_) ,simGetIkGroupHandle(std::get (params_).c_str()) ); - + /// @todo TODO(ahundt) move these functions/member variables into params_ object, take as parameters from lua! linkNames = jointToLink(std::get(params_)); getHandles(linkNames, std::back_inserter(linkHandles)); @@ -217,33 +215,33 @@ void construct() { bool getState(State& state){ if(!allHandlesSet) return false; const std::vector& jointHandle = std::get(handleParams_); - + std::get (state).resize(jointHandle.size()); std::get (state).resize(jointHandle.size()); std::get (state).resize(jointHandle.size()); std::get (state).resize(jointHandle.size()); std::get (state).resize(jointHandle.size()); std::get (state).resize(jointHandle.size()); - + enum limit { lower ,upper ,numLimits }; - + simBool isCyclic; float jointAngleInterval[2]; // min,max double inf = std::numeric_limits::infinity(); - + for (std::size_t i=0 ; i < jointHandle.size() ; i++) - { + { int currentJointHandle = jointHandle[i]; simGetJointPosition(currentJointHandle,&std::get(state)[i]); //retrieves the intrinsic position of a joint (Angle for revolute joint) simGetJointForce(currentJointHandle,&std::get(state)[i]); //retrieves the force or torque applied to a joint along/about its active axis. This function retrieves meaningful information only if the joint is prismatic or revolute, and is dynamically enabled. simGetJointTargetPosition(currentJointHandle,&std::get(state)[i]); //retrieves the target position of a joint simGetJointMatrix(currentJointHandle,&std::get(state)[i][0]); //retrieves the intrinsic transformation matrix of a joint (the transformation caused by the joint movement) simGetJointInterval(currentJointHandle,&isCyclic,jointAngleInterval); - + /// @todo TODO(ahundt) is always setting infinity if it is cyclic the right thing to do? if(isCyclic) { @@ -257,18 +255,18 @@ bool getState(State& state){ } } - + // BOOST_LOG_TRIVIAL(info) << "simJointPostition = " << simJointPosition << std::endl; // BOOST_LOG_TRIVIAL(info) << "simJointForce = " << simJointForce << std::endl; // BOOST_LOG_TRIVIAL(info) << "simJointTargetPostition = " << simJointTargetPosition << std::endl; // BOOST_LOG_TRIVIAL(info) << "simJointTransformationMatrix = " << simJointTransformationMatrix << std::endl; -// +// // float simTipPosition[3]; // float simTipOrientation[3]; // // simGetObjectPosition(target, targetBase, simTipPosition); // simGetObjectOrientation(target, targetBase, simTipOrientation); -// +// // for (int i = 0 ; i < 3 ; i++) // { // BOOST_LOG_TRIVIAL(info) << "simTipPosition[" << i << "] = " << simTipPosition[i] << std::endl; @@ -305,31 +303,31 @@ bool setState(State& state) { for (int i=0 ; i < 7 ; i++) { simSetJointPosition(jointHandle[i],realJointPosition[i]); //Sets the intrinsic position of a joint. May have no effect depending on the joint mode - - + + //simSetJointTargetPosition(jointHandle[i],realJointTargetPosition[i]); //Sets the target position of a joint if the joint is in torque/force mode (also make sure that the joint's motor and position control are enabled - + //simSetJointForce(jointHandle[i],realJointForce[i]); //Sets the maximum force or torque that a joint can exert. This function has no effect when the joint is not dynamically enabled - - + + //simSetJointTargetVelocity(jointHandle[i],realJointTargetVelocity[i]); //Sets the intrinsic target velocity of a non-spherical joint. This command makes only sense when the joint mode is: (a) motion mode: the joint's motion handling feature must be enabled (simHandleJoint must be called (is called by default in the main script), and the joint motion properties must be set in the joint settings dialog), (b) torque/force mode: the dynamics functionality and the joint motor have to be enabled (position control should however be disabled) } - + if (externalHandlesSet) { - + } - + else { - - for (int i=0 ; i < 7 ; i++) { + + for (int i=0 ; i < externalJointForce.size(); i++) { std::string torqueString = boost::lexical_cast(externalJointForce[i]); char * externalTorqueBytes = new char[torqueString.length()+1]; std::strcpy(externalTorqueBytes, torqueString.c_str()); - + simAddObjectCustomData(jointHandle[i], externalTorqueHandle, externalTorqueBytes, torqueString.length()+1); } } - + return true; } diff --git a/include/thirdparty/cartographer/include/cartographer/common/time.h b/include/thirdparty/cartographer/include/cartographer/common/time.h index 31c4778..64648a5 100644 --- a/include/thirdparty/cartographer/include/cartographer/common/time.h +++ b/include/thirdparty/cartographer/include/cartographer/common/time.h @@ -37,15 +37,21 @@ struct UniversalTimeScaleClock { using time_point = std::chrono::time_point; static constexpr bool is_steady = true; - static time_point now() noexcept - { + static time_point now() noexcept + { using namespace std::chrono; + // std::chrono::time_point p1 = std::chrono::system_clock::now(); + // std::chrono::seconds sec(kUtsEpochOffsetFromUnixEpochInSeconds); + // int64 microsecsinceepoch = std::chrono::duration_cast(p1.time_since_epoch()).count(); + // int64 microsec = std::chrono::microseconds(sec).count()*10; + // std::cout << "microseconds since epoch: " << microsecsinceepoch << '\n'; + // std::cout << "microseconds since the Epoch which is January 1, 1 at the start of day in UTC: " + // << microsecsinceepoch - microsec << '\n'; return time_point - ( - duration_cast(system_clock::now().time_since_epoch()) - - seconds(kUtsEpochOffsetFromUnixEpochInSeconds) - ); - } + ( + duration_cast(system_clock::now().time_since_epoch()) + ); + } }; // Represents Universal Time Scale durations and timestamps which are 64-bit diff --git a/include/thirdparty/fbs_tk/fbs_tk.hpp b/include/thirdparty/fbs_tk/fbs_tk.hpp new file mode 100644 index 0000000..4eb332e --- /dev/null +++ b/include/thirdparty/fbs_tk/fbs_tk.hpp @@ -0,0 +1,431 @@ +#ifndef _FBS_TK_HPP_ +#define _FBS_TK_HPP_ +#include +#include + +#include +#include +#include +#include + + +/** + * Add equality to flatbuffers::String + */ +namespace flatbuffers{ +inline bool string_eq(const flatbuffers::String &str1, const flatbuffers::String &str2) { + return str1.str() == str2.str(); +} +inline bool operator==(const flatbuffers::String &str1, const flatbuffers::String &str2) { + return string_eq(str1, str2); +} +inline bool operator!=(const flatbuffers::String &str1, const flatbuffers::String &str2) { + return !string_eq(str1, str2); +} +} + + +namespace fbs_tk { +// http://stackoverflow.com/questions/13037490/ +template +struct range +{ + range(std::istream& in): d_in(in) {} + std::istream& d_in; +}; + +template +std::istream_iterator begin(fbs_tk::range r) { + return std::istream_iterator(r.d_in); +} + +template +std::istream_iterator end(fbs_tk::range) { + return std::istream_iterator(); +} + +// Loads all the contents of the input stream into a string +inline bool load_buffer(std::istream &in, std::string *buf) { + *buf = std::string(std::istreambuf_iterator(in), + std::istreambuf_iterator()); + return !in.bad(); +} + +// Copies a buffer into a string + +inline void buffer_copy(const void *data, size_t size, std::string &dst) { + dst.resize(size); + memcpy(&dst[0], data, size); +} + +struct Buffer { + Buffer() : data() {} + + Buffer(std::size_t size) : data(size) {} + + Buffer(std::initializer_list args) : data(args) {} + + Buffer(std::vector data) : data(std::move(data)) {} + + Buffer(const std::string &str) : data(str.begin(), str.end()) {} + + Buffer(const Buffer &buff) : Buffer(buff.data) {} + + inline void copy_from(const void *buff, size_t buff_size) { + data.resize(buff_size); + memcpy(data.data(), buff, buff_size); + } + + inline void copy_from(const std::string &buff) { + copy_from(buff.data(), buff.size()); + } + + inline void copy_to(void *buff) const { + memcpy(buff, data.data(), data.size()); + } + + inline std::vector &get_data() { + return data; + } + + inline const std::vector &get_data() const { + return data; + } + + inline std::string str() const { + return std::string(data.begin(), data.end()); + } + + inline bool operator==(const Buffer &other) const { + return data == other.data; + } + + inline size_t size() const { + return data.size(); + } + + /** + * Writes all data in this buffer into the output stream. + */ + inline void write_data(std::ostream &out) const { + out.write(reinterpret_cast(data.data()), data.size()); + } + + /** + * Clears the data in the current buffer and then reads a certain number + * of bytes into this buffer. + */ + inline void read_data(std::istream &in, size_t size) { + data.clear(); + data.resize(size); + in.read(reinterpret_cast(data.data()), size); + } + + /** + * Loads all data available in the input stream into this buffer, + * replacing the contents of this buffer. + */ + inline std::istream & read_all_data(std::istream &in) { + data.clear(); + // make sure we do not skip precious bytes + // http://stackoverflow.com/questions/8075795/ + in >> std::noskipws; + data.assign(std::istream_iterator(in), std::istream_iterator()); + return in; + } + + +private: + std::vector data; + + friend std::istream &operator>>(std::istream &in, Buffer &buff) { + char size_buff[sizeof(uint32_t)]; + in.read(size_buff, sizeof(size_buff)); + if (! in) { + return in; + } + auto size = flatbuffers::ReadScalar(size_buff); + buff.read_data(in, size); + return in; + } + + friend std::ostream &operator<<(std::ostream &out, const Buffer &buff) { + uint32_t size = buff.size(); + char size_buff[sizeof(uint32_t)]; + flatbuffers::WriteScalar(size_buff, size); + out.write(size_buff, sizeof(uint32_t)); + buff.write_data(out); + return out; + } +}; // Struct Buffer + +inline void copy_from(Buffer &buffer, const flatbuffers::FlatBufferBuilder &builder) { + buffer.copy_from(builder.GetBufferPointer(), builder.GetSize()); +} + + +// converts a json object into a binary FBS +bool json_to_bin(flatbuffers::Parser &parser, const char *js, Buffer &bin); +// converts a binary FBS into a json object +std::string bin_to_json(flatbuffers::Parser &parser, const Buffer &bin); + +// converts a json object into a binary FBS +bool json_to_fbs(flatbuffers::Parser &parser, std::istream &in, std::ostream &out); +// converts a binary FBS into a json object +bool fbs_to_json(flatbuffers::Parser &parser, std::istream &in, std::ostream &out); + +// converts a stream of binary FBS into a stream of JSON objects +bool fbs_stream_to_jsonl(const std::string &schema, std::istream &in, std::ostream &out); +// converts a stream of JSON objects into a stream of binary FBS +bool jsonl_to_fbs_stream(const std::string &schema, std::istream &in, std::ostream &out); + +// GetRoot from a string +template +inline const T* get_root(const Buffer &buff) { + if (buff.size() == 0) { + // when the buffer is empty, buff.get_data().data() might not + // be initialized + return nullptr; + } + auto data = buff.get_data().data(); + auto check = flatbuffers::Verifier(data, buff.size()); + return check.VerifyBuffer() ? flatbuffers::GetRoot(data) : nullptr; +} + +template +inline const T* get_root_unsafe(const Buffer &buff) { + return flatbuffers::GetRoot(buff.get_data().data()); +} + + // Copy a FBS object. +template +struct copy { + flatbuffers::Offset operator()(flatbuffers::FlatBufferBuilder &, const T&) const; +}; + +// Create a root object and bundle it with its data +template +struct Root { + Root() : root(nullptr), data() {} + + Root(Buffer buff) : data(std::move(buff)) { + // TODO(ahundt) re-enable safe update by default! + // update_root(); + update_root_unsafe(); + } + + Root(const Root &other) : Root(other.data) {} + + /** + * Finishes the builder with obj as root and + * copies the data from the builder. + */ + Root(flatbuffers::FlatBufferBuilder &builder, flatbuffers::Offset obj) : data() { + builder.Finish(obj); + copy_from(data, builder); + update_root_unsafe(); + } + + const T* operator->() const { + assert(valid()); + return root; + } + + const T& operator*() const { + assert(valid()); + return *root; + } + + bool operator==(const Root &other) const { + assert(valid()); + return *root == *other.root; + } + + bool operator!=(const Root &other) const { + assert(valid()); + return *root != *other.root; + } + + bool valid() const { + return root != nullptr; + } + + const Buffer & get_data() const { + return data; + } + + const Buffer & get_data_safe() const { + assert(valid()); + return data; + } + + void set_data(Buffer buff) { + data = std::move(buff); + update_root(); + } + + inline void update_root() { + root = get_root(data); + } + + inline void update_root_unsafe() { + root = get_root_unsafe(data); + } +private: + + friend std::istream &operator>>(std::istream &in, Root &root) { + in >> root.data; + root.update_root(); + if (!root.valid()) { + // mark the input stream as invalid + in.setstate(in.badbit); + } + return in; + } + + friend std::ostream &operator<<(std::ostream &out, const Root &root) { + assert(root.valid()); + out << root.data; + return out; + } + + const T* root; + Buffer data; +}; // namespace Root + +namespace root { + /** + * Copies a FBS object as a root object. + */ + template + static inline Root copy(const S& other) { + flatbuffers::FlatBufferBuilder b; + return Root(b, fbs_tk::copy()(b, other)); + } + + /** + * Copies a root object as a shared_copy of a root object. + */ + template + static inline std::shared_ptr> copy_shared(const Root &obj) { + return std::make_shared>(obj); + } + + /** + * Copies a FBS object as a shared root object. + */ + template + static inline std::shared_ptr> copy_shared(const S &obj) { + return root::copy_shared(root::copy(obj)); + } +} + +template +Root open_root(std::string filename) { + std::ifstream ifs(filename, std::ifstream::binary); + if (!ifs.is_open()) { + return Root(); + } + Buffer buff; + buff.read_all_data(ifs); + fbs_tk::Root result(buff); + if (ifs.bad()) { + ifs.close(); + return Root(); + } + ifs.close(); + return result; +} + +} // namespace fbs_tk + +// Define hash code for Root objects +namespace std { +template +struct hash> { + std::size_t operator()(fbs_tk::Root const& obj) const { + return std::hash()(*obj); + } +}; +} // namespace std + +#include "thirdparty/fbs_tk/fbs_tk.hpp" +#include + +using namespace std; +using namespace flatbuffers; + + +namespace fbs_tk { + +bool json_to_bin(flatbuffers::Parser &parser, const char *js, Buffer &bin) { + + if (!parser.Parse(static_cast(js))) { + return false; + } + copy_from(bin, parser.builder_); + return true; +} + +bool json_to_fbs(flatbuffers::Parser &parser, std::istream &in, std::ostream &out) { + string js; + if (!load_buffer(in, &js)) { + return false; + } + Buffer bin; + if (!json_to_bin(parser, js.c_str(), bin)) { + return false; + } + bin.write_data(out); + return true; +} + +string bin_to_json(flatbuffers::Parser &parser, const Buffer &bin) { + string buffer; + GenerateText(parser, reinterpret_cast(bin.get_data().data()), &buffer); + return buffer; +} + +// converts a binary FBS into a json object +bool fbs_to_json(flatbuffers::Parser &parser, std::istream &in, std::ostream &out) { + Buffer bin; + bin.read_all_data(in); + if (in.bad()) { + return false; + } + out << bin_to_json(parser, bin); + return out.good(); +} + +bool fbs_stream_to_jsonl(const string &schema, istream &in, ostream &out) { + flatbuffers::Parser parser; + parser.Parse(schema.c_str()); + for (const auto & buff : range(in)) { + out << bin_to_json(parser, buff) << endl; + } + return in.eof(); +} + +bool jsonl_to_fbs_stream(const string &schema, istream &in, ostream &out) { + flatbuffers::Parser parser; + parser.Parse(schema.c_str()); + string js; + Buffer bin; + while(getline(in, js)) { + if (!json_to_bin(parser, js.c_str(), bin)) { + return false; + } + out << bin; + if (out.bad()) { + return false; + } + } + return in.eof(); +} + +} // NAMESPACE + + + +//} // namespace std +#endif // _FBS_TK_HPP_ diff --git a/src/java/grl/src/RoboticsAPI.data.xml b/src/java/grl/src/RoboticsAPI.data.xml index 2e70eb4..a5b2d9c 100755 --- a/src/java/grl/src/RoboticsAPI.data.xml +++ b/src/java/grl/src/RoboticsAPI.data.xml @@ -1,41 +1,42 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/java/grl/src/grl/FRIMode.java b/src/java/grl/src/grl/FRIMode.java index e772d2a..3cadc2b 100755 --- a/src/java/grl/src/grl/FRIMode.java +++ b/src/java/grl/src/grl/FRIMode.java @@ -22,15 +22,17 @@ import com.kuka.task.ITaskLogger; /** - * + * * Activate FRI mode in a separate thread * */ + // Provide a Runnable object. The Runnable interface defines a single method, run, meant to contain the code executed in the thread. + // The Runnable object is passed to the Thread constructor, public class FRIMode implements Runnable { - private LBR _lbr; + private LBR _lbr; + - private AbstractMotionControlMode _activeMotionControlMode; private FRISession _friSession = null; @@ -41,13 +43,13 @@ public class FRIMode implements Runnable { private volatile boolean isEnableEnded; private volatile boolean stop = false; private volatile boolean timedOut = false; - + IMotionContainer currentMotion = null; ITaskLogger _logger = null; int iter = 0; /** - * + * * @param flangeAttachment * @param maxAllowedJoints * @param minAllowedJoints @@ -61,6 +63,12 @@ public FRIMode(LBR lbr, String hostName, int sendPeriodMillisec) { _friConfiguration = FRIConfiguration.createRemoteConfiguration(_lbr, _hostName); _friConfiguration.setSendPeriodMilliSec(sendPeriodMillisec); + /** + * Using an instance of the class FRISession, the FRI connection between the + * robot controller and external system is opened and Monitor mode is automatically activated. + * If Monitor mode is activated, the connection quality is continuously determined + * and evaluated. + */ if(_friSession == null) _friSession = new FRISession(_friConfiguration); //_motionOverlay = new FRIJointOverlay(_friSession); } @@ -72,21 +80,27 @@ public void setLogger(ITaskLogger logger) { public boolean isCommandingWaitOrActive() { boolean ret; + /** + * The method getFRIChannelInformation() can be used to poll the following information, the FRI connection and FRI state, + * and save it in a variable of type FRIChannelInformation. + * Type: FRIChannelInformation + */ + synchronized (this) { ret = _friSession.getFRIChannelInformation().getFRISessionState().compareTo(FRISessionState.COMMANDING_ACTIVE) != 0 && _friSession.getFRIChannelInformation().getFRISessionState().compareTo(FRISessionState.COMMANDING_WAIT) != 0; } - + return !ret; } - + public String getQualityString() { return _friSession.getFRIChannelInformation().getQuality().toString(); } - + /** - * + * * @param setControlMode */ public void setControlMode(AbstractMotionControlMode teachModeControlMode) { @@ -95,7 +109,7 @@ public void setControlMode(AbstractMotionControlMode teachModeControlMode) { _activeMotionControlMode = teachModeControlMode; } } - + public void setFRIConfiguration(FRIConfiguration friConfguration){ _friConfiguration = friConfguration; } @@ -106,10 +120,10 @@ public void enable() { useHandGuidingMotion = true; } } - + /** * Tell the hand guiding motion (teach mode) to stop - * and the thread exits. Make sure + * and the thread exits. Make sure * isEnableEnded returns true before calling this! * @return false on failure; true on successfully starting the shutdown process */ @@ -118,36 +132,36 @@ public synchronized boolean stop(){ stop = true; if(currentMotion !=null) currentMotion.cancel(); - + return true; } - + public synchronized boolean isTimedOut(){ return timedOut; } - + /** - * + * * Cancel the motion, thread stays in existence. */ public boolean cancel(){ //warn("Cancel received"); - + synchronized (this) { useHandGuidingMotion = false; - + if(currentMotion !=null) currentMotion.cancel(); } return true; //warn("Cancel complete"); } - + /** * @brief true there are no outstanding calls made to enable() and you can switch modes - * + * * @see enable() - * + * * The sunrise HandGuidingMotion mode has an API * quirk where you need to push the physical button * to end the hand guiding mode. This tells you if @@ -178,7 +192,7 @@ public void run() { useHandGuidingMotion = false; } _motionOverlay = null; - + while(!stop && !timedOut) { //warn("Starting new hand guiding motion"); @@ -201,7 +215,7 @@ public void run() { } continue; } - + warn("creating FRI Joint Overlay " + useHandGuidingMotion); isEnableEnded = false; _motionOverlay = new FRIJointOverlay(_friSession); @@ -227,7 +241,7 @@ public void run() { _motionOverlay = new FRIJointOverlay(_friSession); currentMotion = _lbr.move(positionHold(_activeMotionControlMode, -1, TimeUnit.SECONDS).addMotionOverlay(_motionOverlay)); _logger.info("FRI Joint Overlay ended..."); - + } catch (TimeoutException e) { //_logger.error("FRISession timed out, closing..."); //e.printStackTrace(); diff --git a/src/java/grl/src/grl/ProcessDataManager.java b/src/java/grl/src/grl/ProcessDataManager.java index 9c0aaba..7a0506f 100755 --- a/src/java/grl/src/grl/ProcessDataManager.java +++ b/src/java/grl/src/grl/ProcessDataManager.java @@ -21,6 +21,7 @@ public class ProcessDataManager { private String _FRI_KONI_LaptopIPAddress; private String _ROS_MASTER_URI; private String _ZMQ_MASTER_URI; + private int _FRI_KONI_SendPeriodMilliseconds; private double _jointVelRel; private double _jointAccelRel; @@ -75,11 +76,6 @@ private void update_ZMQ_MASTER_URI() { this._ZMQ_MASTER_URI = "tcp://" + _controllingLaptopIPAddress + ":" + _controllingLaptopZMQPort; } - - public String get_ROS_MASTER_URI() { - return _ROS_MASTER_URI; - } - private void update_ROS_MASTER_URI() { this._ROS_MASTER_URI = "http://" + _controllingLaptopIPAddress + ":" + _controllingLaptop_ROS_MASTER_URI_Port; } @@ -98,6 +94,11 @@ public String get_controllingLaptopJAVAPort() { return _controllingLaptopZMQPort; } + + public int get_FRI_KONI_SendPeriodMilliseconds() { + return _FRI_KONI_SendPeriodMilliseconds; + } + public void set_controllingLaptopJAVAPort(String _controllingLaptopJAVAPort) { this._controllingLaptopZMQPort = _controllingLaptopJAVAPort; update_ZMQ_MASTER_URI(); @@ -120,9 +121,19 @@ public void set_RobotIPAddress(String _RobotIPAddress) { this._RobotIPAddress = _RobotIPAddress; } + + public void set_FRI_KONI_SendPeriodMilliseconds(int FRI_KONI_SendPeriodMilliseconds) { + this._FRI_KONI_SendPeriodMilliseconds = FRI_KONI_SendPeriodMilliseconds; + } + public String get_FRI_KONI_RobotIPAddress() { return _FRI_KONI_RobotIPAddress; } + + public String get_ROS_MASTER_URI() { + return _ROS_MASTER_URI; + } + public void set_FRI_KONI_RobotIPAddress(String _FRI_KONI_RobotIPAddress) { this._FRI_KONI_RobotIPAddress = _FRI_KONI_RobotIPAddress; @@ -221,6 +232,8 @@ public ProcessDataManager(final RoboticsAPIApplication app){ // ********************************************************************** _FRI_KONI_RobotIPAddress = _app.getApplicationData().getProcessData("Robot_KONI_FRI_IP").getValue(); //"tcp://172.31.1.100:30010"; + _FRI_KONI_SendPeriodMilliseconds = _app.getApplicationData().getProcessData("FRI_KONI_SendPeriodMilliseconds").getValue(); + _jointVelRel = _app.getApplicationData().getProcessData("JointVelRel").getValue(); _jointAccelRel = _app.getApplicationData().getProcessData("JointAccelRel").getValue(); diff --git a/src/java/grl/src/grl/UDPManager.java b/src/java/grl/src/grl/UDPManager.java index 9816806..00ba910 100755 --- a/src/java/grl/src/grl/UDPManager.java +++ b/src/java/grl/src/grl/UDPManager.java @@ -7,15 +7,16 @@ import java.net.*; public class UDPManager { - + + // This class represents a socket for sending and receiving datagram packets. DatagramSocket socket = null; ITaskLogger logger; String _Remote_IP; int _Remote_Port; - + // This class represents an Internet Protocol (IP) address. InetAddress _address_send = null; - + int statesLength = 0; long message_counter = 0; long noMessageCounter = 0; @@ -33,59 +34,64 @@ public class UDPManager { long lastMessageStartTime; long lastMessageElapsedTime; long lastMessageTimeoutMilliseconds = 1000; - + int retriesAllowed = 3; int retriesAttempted = 0; - + public UDPManager(String laptopIP, String laptopPort, ITaskLogger errorlogger) { super(); this.logger = errorlogger; _Remote_IP = laptopIP; _Remote_Port = Integer.parseInt(laptopPort); - + try { _address_send = InetAddress.getByName(_Remote_IP); } catch (UnknownHostException e) { logger.error("Could not create InetAddress for sending"); } - + } /** * Blocks until a connection is established or stop() is called. - * + * * @return error code: false on success, otherwise failure (or told to stop) - * @throws UnknownHostException + * @throws UnknownHostException */ public boolean connect() { logger.info("Waiting for connection initialization..."); - + try { socket = new DatagramSocket(); } catch (SocketException e1) { logger.info("failed to create socket."); } - - + + try { + // Enable/disable SO_TIMEOUT with the specified timeout, in milliseconds. + // With this option set to a non-zero timeout, a read() call on the InputStream associated with this Socket will block for only this amount of time. If the timeout expires, a java.net.SocketTimeoutException is raised, socket.setSoTimeout(100); } catch (SocketException e1) { logger.error("UDPManager failed to set socket timeout"); } - + startTime = System.currentTimeMillis(); elapsedTime = 0L; grl.flatbuffer.KUKAiiwaStates newKUKAiiwaStates = null; - int newStatesLength = 0; - + int newStatesLength = 0; + boolean connectionEstablished = false; - + while(newStatesLength<1 && newKUKAiiwaStates == null){ - + // Datagram packets are used to implement a connectionless packet delivery service. + // Constructs a DatagramPacket for receiving packets of length length. + // recBuf - buffer for holding the incoming datagram. + // DatagramPacket packet = new DatagramPacket(recBuf, recBuf.length); - + // continues sending dummy messages until the server receives the address of this machine and sends a message back while (!connectionEstablished){ connectionEstablished = preConnect(); @@ -94,14 +100,14 @@ public boolean connect() { return true; // asked to exit } } - - if(packet.getLength() > 0){ + if(packet.getLength() > 0){ + // Wraps a byte array into a buffer. bb = ByteBuffer.wrap(recBuf); - + newKUKAiiwaStates = grl.flatbuffer.KUKAiiwaStates.getRootAsKUKAiiwaStates(bb); newStatesLength = newKUKAiiwaStates.statesLength(); - + } if (stop) { @@ -109,12 +115,12 @@ public boolean connect() { return true; // asked to exit } } - + _currentKUKAiiwaStates = newKUKAiiwaStates; statesLength = newStatesLength; logger.info("States initialized..."); - + startTime = System.currentTimeMillis(); lastMessageStartTime = startTime; lastMessageElapsedTime = System.currentTimeMillis() - lastMessageStartTime; @@ -122,14 +128,14 @@ public boolean connect() { return false; // no error } - + /// @return true successfully sent and received a message, false otherwise private boolean preConnect() { // Dummy message to send to Remote pc (server), in order for the server to know the address of the client (this machine) /// @todo Should probably just make this a real flatbuffers init message too String dummyMessage = "Hi"; - + DatagramPacket packetSend= new DatagramPacket(dummyMessage.getBytes(), dummyMessage.getBytes().length, _address_send, _Remote_Port); try { @@ -142,7 +148,7 @@ private boolean preConnect() socket.receive(packet); return true; } catch (SocketTimeoutException e) { - // TimeOut reached, continue sending until we receive something + // TimeOut reached, continue sending until we receive something return false; } catch (IOException e) { // Could not receive packet @@ -152,24 +158,24 @@ private boolean preConnect() /** * Blocks until a connection is re-established or stop() is called. - * + * * @return error code: false on success, otherwise failure (or told to stop) - * @throws IOException + * @throws IOException */ - + public boolean sendMessage(byte[] msg, int size) throws IOException { DatagramPacket packet= new DatagramPacket(msg, size, _address_send , _Remote_Port ); socket.send(packet); return true; } - + public grl.flatbuffer.KUKAiiwaState waitForNextMessage() { boolean haveNextMessage = false; while(!stop && !haveNextMessage) { - + DatagramPacket packet = new DatagramPacket(recBuf, recBuf.length); try { socket.receive(packet); @@ -178,9 +184,9 @@ public grl.flatbuffer.KUKAiiwaState waitForNextMessage() message_counter+=1; bb = ByteBuffer.wrap(recBuf); - + _currentKUKAiiwaStates = grl.flatbuffer.KUKAiiwaStates.getRootAsKUKAiiwaStates(bb, _currentKUKAiiwaStates); - + if(_currentKUKAiiwaStates.statesLength()>0) { // initialize the fist state grl.flatbuffer.KUKAiiwaState tmp = _currentKUKAiiwaStates.states(0); @@ -194,13 +200,13 @@ public grl.flatbuffer.KUKAiiwaState waitForNextMessage() _previousKUKAiiwaState = _currentKUKAiiwaState; _currentKUKAiiwaState = tmp; } - + if (_currentKUKAiiwaState == null) { noMessageCounter+=1; logger.error("Missing current state message!"); continue; } - + haveNextMessage=true; noMessageCounter = 0; lastMessageStartTime = System.currentTimeMillis(); @@ -222,22 +228,22 @@ public grl.flatbuffer.KUKAiiwaState waitForNextMessage() preConnect(); } } - + } - - return _currentKUKAiiwaState; + + return _currentKUKAiiwaState; } - + public grl.flatbuffer.KUKAiiwaState getCurrentMessage(){ return _currentKUKAiiwaState; } - + public grl.flatbuffer.KUKAiiwaState getPrevMessage(){ return _previousKUKAiiwaState; } - - + + public boolean isStop() { return stop; } @@ -249,14 +255,14 @@ public void setStop(boolean stop) { socket.close(); logger.error("socket closed"); this.stop = stop; - + } } - + public void stop(){ setStop(true); } - + protected void finalize() { try { logger.error("Trying to close socket"); @@ -269,9 +275,9 @@ protected void finalize() { } } - - - - - + + + + + } diff --git a/src/java/grl/src/grl/UpdateConfiguration.java b/src/java/grl/src/grl/UpdateConfiguration.java index 583254e..cac8213 100755 --- a/src/java/grl/src/grl/UpdateConfiguration.java +++ b/src/java/grl/src/grl/UpdateConfiguration.java @@ -51,12 +51,12 @@ public void set_FRISession(FRISession _FRISession) { } private FRISession _FRISession = null; - + public UpdateConfiguration(LBR lbr, Tool flangeAttachment){ _lbr = lbr; _flangeAttachment = flangeAttachment; } - + /** * Create an End effector tool * @param link the Flatbuffer representing the tool @@ -81,7 +81,7 @@ public PhysicalObject linkObjectToPhysicalObject(grl.flatbuffer.LinkObject link link.inertia().mass(), centerOfMassInMillimeter); } - + /** * Sets the FRI Configuration and FRI Session stored by the UpdateConfiguration object * @param friConfigBuf flatbuffer containing fri settings @@ -95,11 +95,18 @@ public boolean FRIflatbufferToConfigAndSession (grl.flatbuffer.FRI friConfigBuf, if(friConfigBuf !=null && friConfigBuf.sendPeriodMillisec() >0){ _FRIConfiguration.setSendPeriodMilliSec(friConfigBuf.sendPeriodMillisec()); + /** + * Defines the answer rate factor (type: int) + * The answer rate factor defines the rate at which the external + * system is to send data to the robot controller: + * Answer rate = answer rate factor*send rate + */ + _FRIConfiguration.setReceiveMultiplier(friConfigBuf.setReceiveMultiplier()); } - + _FRISession = new FRISession(_FRIConfiguration); - + return false; } diff --git a/src/java/grl/src/grl/driver/GRL_Driver.java b/src/java/grl/src/grl/driver/GRL_Driver.java old mode 100644 new mode 100755 index b23b3c6..83263f4 --- a/src/java/grl/src/grl/driver/GRL_Driver.java +++ b/src/java/grl/src/grl/driver/GRL_Driver.java @@ -228,7 +228,7 @@ public void initialize() _teachModeThread.start(); - int millisecondsPerFRITimeStep = 4; + int millisecondsPerFRITimeStep = _processDataManager.get_FRI_KONI_SendPeriodMilliseconds(); _FRIModeRunnable = new FRIMode( _lbr, _processDataManager.get_FRI_KONI_LaptopIPAddress(), millisecondsPerFRITimeStep); @@ -869,6 +869,8 @@ private void switchSmartServoMotion(byte controlMode) { if (!(_smartServoMotion.getMode() instanceof PositionControlMode)) { // We are in PositioControlMode and the request was for the same mode, there are no parameters to change. getLogger().info("Changing parameters of Smart Servo in " + grl.flatbuffer.EControlMode.name(controlMode)); + // With the changeControlModeSettings(...) method, it is possible to change the controller parameters with which the + // servo motion was started. The control type itself cannot be changed during the servo motion. _smartServoMotion.getRuntime().changeControlModeSettings(getMotionControlMode(controlMode, defaultParams)); configureSmartServoLock.unlock(); return; diff --git a/src/java/grl/src/grl/driver/ZMQ_SmartServoFRI.java b/src/java/grl/src/grl/driver/ZMQ_SmartServoFRI.java index bf591f9..63a2c1e 100755 --- a/src/java/grl/src/grl/driver/ZMQ_SmartServoFRI.java +++ b/src/java/grl/src/grl/driver/ZMQ_SmartServoFRI.java @@ -25,6 +25,7 @@ public class ZMQ_SmartServoFRI extends RoboticsAPIApplication { private Controller _lbrController; + // Robot instance in the application private LBR _lbr; private String _hostName; private String _controllingLaptopIPAddress; @@ -44,7 +45,7 @@ public void initialize() // *** change next line to the KUKA address and Port Number *** // ********************************************************************** _controllingLaptopIPAddress = "tcp://172.31.1.100:30010"; - + // FIXME: Set proper Weights or use the plugin feature double[] translationOfTool = @@ -61,22 +62,27 @@ public void initialize() public void run() { // Configure and start FRI session + /** + * The FRI connection to an external system is configured via the static method + * createRemoteConfiguration(robot ,IP address). The method belongs to the class FRIConfiguration. + **/ FRIConfiguration friConfiguration = FRIConfiguration.createRemoteConfiguration(_lbr, _hostName); + // Define the send rate (type: int, unit ms) friConfiguration.setSendPeriodMilliSec(4); FRISession friSession = new FRISession(friConfiguration); - + // TODO: remove default start pose // move do default start pose _toolAttachedToLBR.move(ptp(Math.toRadians(10), Math.toRadians(10), Math.toRadians(10), Math.toRadians(-90), Math.toRadians(10), Math.toRadians(10),Math.toRadians(10))); - + // Prepare ZeroMQ context and dealer ZMQ.Context context = ZMQ.context(1); ZMQ.Socket subscriber = context.socket(ZMQ.DEALER); subscriber.connect(_controllingLaptopIPAddress); subscriber.setRcvHWM(1000000); - - + + JointPosition initialPosition = new JointPosition( _lbr.getCurrentJointPosition()); @@ -89,27 +95,27 @@ public void run() aSmartServoMotion.setMinimumTrajectoryExecutionTime(20e-3); _toolAttachedToLBR.getDefaultMotionFrame().moveAsync(aSmartServoMotion); - + // Fetch the Runtime of the Motion part theSmartServoRuntime = aSmartServoMotion.getRuntime(); - + // create an JointPosition Instance, to play with JointPosition destination = new JointPosition( _lbr.getJointCount()); - - + + byte [] data = subscriber.recv(); ByteBuffer bb = ByteBuffer.wrap(data); JointState jointState = JointState.getRootAsJointState(bb); - + // move to start pose //_lbr.move(ptp(jointState.position(0), jointState.position(1), jointState.position(2), jointState.position(3), jointState.position(4), jointState.position(5), jointState.position(6))); //.setJointAccelerationRel(acceleration)); - - + + // Receive Flat Buffer and Move to Position // TODO: make into while loop and add exception to exit loop // TODO: add a message that we send to the driver with data log strings @@ -120,7 +126,7 @@ public void run() bb = ByteBuffer.wrap(data); jointState = JointState.getRootAsJointState(bb); - + for (int k = 0; k < destination.getAxisCount(); ++k) { destination.set(k, jointState.position(k)); @@ -134,7 +140,7 @@ public void run() //_lbr.moveAsync(ptp(jointState.position(0), jointState.position(1), jointState.position(2), jointState.position(3), jointState.position(4), jointState.position(5), jointState.position(6))); //.setJointAccelerationRel(acceleration)); } - + // done subscriber.close(); context.term(); @@ -144,7 +150,7 @@ public void run() /** * main. - * + * * @param args * args */ diff --git a/src/lua/robone.lua b/src/lua/robone.lua index 7491b10..f2113df 100644 --- a/src/lua/robone.lua +++ b/src/lua/robone.lua @@ -5,9 +5,15 @@ --- require "grl" --- 3) call your function! ex: grl.isModuleLoaded('') +--- LRB_iiwa_14_R820#0 represents the real robot; +--- LRB_iiwa_14_R820 represents the robot for simulation + robone = {} require "grl" + +KUKA_single_buffer_limit_bytes = 256 -- MB +FB_single_buffer_limit_bytes = 1024 -- MB ------------------------------------------ -- Move the arm along the cut file path -- ------------------------------------------ @@ -56,70 +62,72 @@ robone.cutBoneScript=function() --maxVel = 0.04 --maxForce = 30 - while true do - if runFollowPathMode then - -- Moves an object to the position/orientation of another moving object (target object) - -- by performing interpolations (i.e. the object will effectiviely follow the target object). - simMoveToObject(target,CreatedPathHandle,3,0,0.5,0.02) - -- http://www.coppeliarobotics.com/helpFiles/en/apiFunctions.htm#simFollowPath - simFollowPath(target,CreatedPathHandle,3,0,0.01,0.01) - --simSwitchThread() - - else - -- Calculate path length - pathLength = simGetPathLength(CreatedPathHandle) - -- Move to start of path - - simMoveToObject(target,CreatedPathHandle,3,0,0.2,0.02) - newPosition = 0 - - - - -- Create empty vector for tool tip forces (x,y,z,alpha,beta,gamma,joint dependence(?)) - --toolTipForces = matrix(7,1,0) - toolTipForces = {0,0,0,0,0,0,0} - newPos = simGetPositionOnPath(CreatedPathHandle,0) - -- While not at end of path, traverse path - while newPosition < pathLength do - print("VREP JOINT POSITIONS") - for i=1,7,1 do - print(simGetJointPosition(jointHandles[i])) - end - externalJointTorque = getExternalJointTorque(measuredJointHandles, externalTorqueHandle) - - measuredForce = calcToolTipForce(IKGroupHandle, externalJointTorque) - print("MEASURED FORCE: "..measuredForce) - - position = simGetPathPosition(CreatedPathHandle) - offset = 0.001 - newPosition = position+offset - - simSetPathPosition(CreatedPathHandle, newPosition) - nextPos = simGetPositionOnPath(CreatedPathHandle, newPosition/pathLength) - nextEul = simGetOrientationOnPath(CreatedPathHandle, newPosition/pathLength) - nextQuat = simGetQuaternionFromMatrix(simBuildMatrix({0,0,0},nextEul)) - nextVel = {0,0,0,0} - for i=1,3,1 do - nextVel[i] = 3*(nextPos[i]-newPos[i]) - end - --timeBefore = simGetSimulationTime() - --print("Time nonRML takes: ".. timeBefore-timeAfter) - --print("Time before RML: "..timeBefore) - result,newPos,newQuat,currentVel,currentAccel,timeLeft=simRMLMoveToPosition(target,-1,-1,currentVel,currentAccel,maxVel,maxAccel,maxJerk,nextPos,nextQuat,nextVel) - --timeAfter = simGetSimulationTime() - --print("Time after RML: "..timeAfter) - --print("Time RML takes: ".. timeAfter-timeBefore) - --print("Time left: "..timeLeft) - - - --fcv(IKGroupHandle, CreatedPathHandle, maxVel, maxForce, measuredForce, pathLength) - - --simSwitchThread() - - end - simSetPathPosition(CreatedPathHandle, 0) - end - end + while true do + if runFollowPathMode then + -- Moves an object ('RobotMillTipTarget') to the position/orientation of another moving object (target object, 'MillHipCutPath' or 'Straingtline') + -- by performing interpolations (i.e. the object will effectiviely follow the target object). + simMoveToObject(target,CreatedPathHandle,3,0,0.5,0.02) + -- http://www.coppeliarobotics.com/helpFiles/en/apiFunctions.htm#simFollowPath + simFollowPath(target,CreatedPathHandle,3,0,0.01,0.01) + --simSwitchThread() + + else + -- Calculate path length + -- Retrieves the length of a path object. + pathLength = simGetPathLength(CreatedPathHandle) + -- Move to start of path + + simMoveToObject(target,CreatedPathHandle,3,0,0.2,0.02) + newPosition = 0 + + -- Create empty vector for tool tip forces (x,y,z,alpha,beta,gamma,joint dependence(?)) + -- toolTipForces = matrix(7,1,0) + toolTipForces = {0,0,0,0,0,0,0} + -- Retrieves the absolute interpolated position of a point along a path object. + -- 0 means the beginning of the path + newPos = simGetPositionOnPath(CreatedPathHandle,0) + -- While not at end of path, traverse path + while newPosition < pathLength do + print("VREP JOINT POSITIONS") + for i=1,7,1 do + print(simGetJointPosition(jointHandles[i])) + end + externalJointTorque = getExternalJointTorque(measuredJointHandles, externalTorqueHandle) + + measuredForce = calcToolTipForce(IKGroupHandle, externalJointTorque) + print("MEASURED FORCE: "..measuredForce) + -- Retrieves the intrinsic position of a path object (a distance along the path). + position = simGetPathPosition(CreatedPathHandle) + offset = 0.001 -- step size + newPosition = position+offset + -- Sets the intrinsic position of a path object (i.e. the position along the path). + simSetPathPosition(CreatedPathHandle, newPosition) + nextPos = simGetPositionOnPath(CreatedPathHandle, newPosition/pathLength) + nextEul = simGetOrientationOnPath(CreatedPathHandle, newPosition/pathLength) + nextQuat = simGetQuaternionFromMatrix(simBuildMatrix({0,0,0},nextEul)) + nextVel = {0,0,0,0} + for i=1,3,1 do + nextVel[i] = 3*(nextPos[i]-newPos[i]) + end + --timeBefore = simGetSimulationTime() + --print("Time nonRML takes: ".. timeBefore-timeAfter) + --print("Time before RML: "..timeBefore) + --[[Moves an object to a given position and/or orientation using the Reflexxes Motion Library type II or IV. This function can only be called from child scripts running in a thread --]] + result,newPos,newQuat,currentVel,currentAccel,timeLeft=simRMLMoveToPosition(target,-1,-1,currentVel,currentAccel,maxVel,maxAccel,maxJerk,nextPos,nextQuat,nextVel) + --timeAfter = simGetSimulationTime() + --print("Time after RML: "..timeAfter) + --print("Time RML takes: ".. timeAfter-timeBefore) + --print("Time left: "..timeLeft) + + + --fcv(IKGroupHandle, CreatedPathHandle, maxVel, maxForce, measuredForce, pathLength) + + --simSwitchThread() + + end + simSetPathPosition(CreatedPathHandle, 0) + end + end --Path0P,Path0O=grl.getTransformToPathPointInWorldFrame(BallJointPath,0) --pathPos=simGetObjectPosition(BallJointPath,targetBase) @@ -150,7 +158,7 @@ robone.cutBoneScript=function() jointHandles={-1,-1,-1,-1,-1,-1,-1} measuredJointHandles={-1,-1,-1,-1,-1,-1,-1} for i=1,7,1 do - jointHandles[i]=simGetObjectHandle('LBR_iiwa_14_R820_joint'..i) + jointHandles[i]=simGetObjectHandle('LBR_iiwa_14_R820_joint'..i) -- Retrieves an object handle based on its name. measuredJointHandles[i]=simGetObjectHandle('LBR_iiwa_14_R820_joint'..i..'#0') end @@ -222,8 +230,7 @@ robone.cutBoneScript=function() end function fcv(ikGroupHandle, CreatedPathHandle, maxVelocity, maxForce, measuredForce, pathLength) - - -- selection parameter for simRMLPos for default behavior + -- selection parameter for simRMLPos for default behavior selection = {1,1,1,1,1,1,1} -- Get position (in meters) on the path (0 = start, pathLength = end) @@ -257,7 +264,7 @@ robone.cutBoneScript=function() end end - target=simGetObjectHandle('RobotMillTipTarget') + target = simGetObjectHandle('RobotMillTipTarget') targetBase=simGetObjectHandle('Robotiiwa') bone=simGetObjectHandle('FemurBone') table=simGetObjectHandle('highTable') @@ -303,7 +310,14 @@ robone.cutBoneScript=function() useRMLSmoothing = false if (grl.isModuleLoaded('GrlInverseKinematics') and useGrlInverseKinematics) then - simExtGrlInverseKinematicsStart() + -- ik_mode, run real inverse kinematics algorith; + -- replay_mode, run the replay process; + -- otherwise, go to a test pose. + -- commanddata, only in replay_mode we need to set it to determine 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.test_mode, commanddata) end print("Moving Robotiiwa arm along cut path...") @@ -346,12 +360,13 @@ robone.handEyeCalibScript=function() path=simGetObjectHandle('HandEyeCalibPath') circleCalib = simGetObjectHandle('CircleCalibPath') endeffectorTarget=simGetObjectHandle('RobotMillTipTarget') - numSteps=36 + numSteps=24 startP,startO=grl.getTransformBetweenHandles(target,targetBase) -- Enable/Disable custom IK - useGrlInverseKinematics=false + useGrlInverseKinematics=true + --simExtHandEyeCalibStart('Robotiiwa' , 'RobotFlangeTipTarget', 'OpticalTrackerBase', 'Fiducial#22') if (grl.isModuleLoaded('GrlInverseKinematics') and useGrlInverseKinematics) then simExtGrlInverseKinematicsStart() @@ -361,14 +376,18 @@ robone.handEyeCalibScript=function() simDisplayDialog('Error','HandEyeCalibration plugin was not found. (v_repExtHandEyeCalibration.dll)&&nSimulation will not run correctly',sim_dlgstyle_ok,true,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1}) else + simWait(30.0) -- Run the hand eye calibration - simExtHandEyeCalibStart() + -- simExtHandEyeCalibStart() + -- call handEyeCalibrationPG->construct(); + simExtHandEyeCalibStart('Robotiiwa' , 'RobotMillTipTarget', 'OpticalTrackerBase', 'Fiducial#22') + -- Fill out the vectors, and then pass these vectors to for i=0,1,1/numSteps do p,o=grl.getPathPointInWorldFrame(path,i) simRMLMoveToPosition(target,-1,-1,currentVel,currentAccel,maxVel,maxAccel,maxJerk,p,o,nil) - simWait(0.5) + simWait(0.2) --0.5 simExtHandEyeCalibAddFrame() simWait(0.25) end @@ -378,7 +397,9 @@ robone.handEyeCalibScript=function() -- calculate the transform simExtHandEyeCalibFindTransform() - simExtHandEyeCalibApplyTransform() + -- apply the transform and save the scene + sceneName = "/home/cjiao1/src/robonetracker/modules/roboneprivate/data/RoboneSimulation_private_calibration_5.ttt" + simExtHandEyeCalibApplyTransform(sceneName) -- check for fusiontrack if (not grl.isModuleLoaded('AtracsysFusionTrack')) then @@ -428,10 +449,12 @@ robone.startRealArmDriverScript=function() '30200' , -- LocalHostKukaKoniUDPPort, '192.170.10.2' , -- RemoteHostKukaKoniUDPAddress, '30200' , -- RemoteHostKukaKoniUDPPort - 'JAVA' , -- KukaCommandMode (options are FRI, JAVA) + 'FRI' , -- KukaCommandMode (options are FRI, JAVA) 'FRI' , -- KukaMonitorMode (options are FRI, JAVA) "IK_Group1_iiwa" -- IKGroupName ) + + simExtKukaLBRiiwaRecordWhileSimulationIsRunning(true, KUKA_single_buffer_limit_bytes) else simDisplayDialog('Error','KukaLBRiiwa plugin was not found. (v_repExtKukaLBRiiwa.dll)&&nSimulation will run without hardware',sim_dlgstyle_ok,true,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1}) end @@ -440,6 +463,7 @@ end ------------------------------------------ -- Configure the Optical Tracker -- -- Call in a "Customization Script" -- +-- The functions in Customization Script are executed all the time: when simulation is running, as well as when simulation is not running. ------------------------------------------ robone.configureOpticalTracker=function() @@ -464,9 +488,14 @@ robone.configureOpticalTracker=function() simExtAtracsysFusionTrackAddGeometry('geometry0022.ini') simAddStatusbarMessage('robone.configureOpticalTracker() + v_repExtAtracsysFusionTrackVrepPlugin: loading geometry0055.ini') simExtAtracsysFusionTrackAddGeometry('geometry0055.ini') + -- Add the new geometry attached to the frame. + -- geometry50000.ini + simAddStatusbarMessage('robone.configureOpticalTracker() + v_repExtAtracsysFusionTrackVrepPlugin: loading geometry50000.ini') + simExtAtracsysFusionTrackAddGeometry('geometry50000.ini') -------------------------------------------------- + -- Move the Tracker--- -- Move the Tracker -- true enables moving the tracker, false disables it moveTracker = false @@ -480,8 +509,18 @@ robone.configureOpticalTracker=function() 'Fiducial#22', -- ObjectBeingMeasured '22' -- GeometryID ) - end + + + -- Set the new marker in vrep based on the Fiducial#22, the relative pose is read from FusionTracker + simExtAtracsysFusionTrackAddObject('Fiducial#50000', -- ObjectToMove + 'OpticalTrackerBase#0', -- FrameInWhichToMoveObject + 'Fiducial#50000', -- ObjectBeingMeasured + '50000' -- GeometryID + ) + + + end -------------------------------------------------- -- Move the Bone -- true enables moving the bone, false disables it @@ -490,19 +529,34 @@ robone.configureOpticalTracker=function() simAddStatusbarMessage('robone.configureOpticalTracker() + v_repExtAtracsysFusionTrackVrepPlugin: Moving bone marker position relative to the optical tracker.') simExtAtracsysFusionTrackClearObjects() -- The bone should move (bone is attached to Fiducial #55) + simExtAtracsysFusionTrackAddObject('Fiducial#55', -- ObjectToMove 'OpticalTrackerBase#0', -- FrameInWhichToMoveObject 'Fiducial#55', -- ObjectBeingMeasured '55' -- GeometryID ) - end + -- Get the transform matrix of robot base against world frame. + -- This is also for the calibration purpose, it can print the absolute position and orientation of the robot base. + robothandle = simGetObjectHandle ('LBR_iiwa_14_R820#0') + --trankerhandle = simGetObjectHandle ('OpticalTrackerBase#0') + robotToWorldM = simGetObjectMatrix(robothandle, -1) + print('The absolute transformation matrix of LBR_iiwa_14_R820#0') + for i=1,12,1 do + print(robotToWorldM[i].." ") + end + end + + -- Start collecting data from the optical tracker simExtAtracsysFusionTrackStart() - simExtAtracsysFusionTrackRecordWhileSimulationIsRunning(false) - + -- Test the below one + -- Enable the recordDataScript() to call the method below. + simExtAtracsysFusionTrackRecordWhileSimulationIsRunning(true, FB_single_buffer_limit_bytes) + end + -- By default we disable customization script execution during simulation, in order -- to run simulations faster: simSetScriptAttribute(sim_handle_self,sim_customizationscriptattribute_activeduringsimulation,false) @@ -510,6 +564,27 @@ robone.configureOpticalTracker=function() end +------------------------------------------------ +-- Record flatbuffers Data during simulation -- +------------------------------------------------ +robone.recordDataScript=function() + + -- Check if the required plugin is there: + if (grl.isModuleLoaded('KukaLBRiiwa')) then + --simExtKukaLBRiiwaRecordWhileSimulationIsRunning(true) + else + simDisplayDialog('Warning','KukaLBRiiwa plugin was not found so data will not be logged.',sim_dlgstyle_ok,true,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1}) + end + + + if (grl.isModuleLoaded('AtracsysFusionTrack')) then + --simExtAtracsysFusionTrackRecordWhileSimulationIsRunning(true) + else + simDisplayDialog('Warning','Atracsys plugin was not found so data will not be logged.',sim_dlgstyle_ok,true,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1}) + + end +end + -------------------------------------------------------------------------- -- Get External Joint Torque Data from KUKA iiwa real arm driver plugin -- -------------------------------------------------------------------------- @@ -540,4 +615,4 @@ robone.getExternalJointTorque=function(dofs, jointHandles, externalTorqueHandle) return externalJointTorque end -return robone \ No newline at end of file +return robone diff --git a/src/ros/CMakeLists.txt b/src/ros/CMakeLists.txt index af03ea3..97af8cd 100644 --- a/src/ros/CMakeLists.txt +++ b/src/ros/CMakeLists.txt @@ -41,9 +41,9 @@ if(ROS_FOUND AND TARGET KukaFRIClient AND Boost_FOUND AND Boost_CHRONO_FOUND) - + basis_include_directories(${FRI-Client-SDK_Cpp_PROJECT_INCLUDE_DIRS} ${FRI-Client-SDK_Cpp_INCLUDE_DIRS} ${ROS_INCLUDE_DIR}) - basis_add_executable(grl_kuka_ros_driver grl_kuka_ros_driver.cpp) + basis_add_executable(grl_kuka_ros_driver grl_kuka_ros_driver.cpp ) basis_add_dependencies(grl_kuka_ros_driver grlflatbuffers ${FRI-Client-SDK_Cpp_LIBRARIES}) basis_target_link_libraries(grl_kuka_ros_driver ${Boost_LIBRARIES} @@ -52,6 +52,7 @@ if(ROS_FOUND AND TARGET KukaFRIClient AND Boost_FOUND AND Boost_CHRONO_FOUND) ${CMAKE_THREAD_LIBS_INIT} ${Nanopb_LIBRARIES} ${ROS_LIBRARIES} + ${FLATBUFFERS_STATIC_LIB} KukaFRIClient ) target_compile_definitions(grl_kuka_ros_driver PUBLIC $) diff --git a/src/v_repExtAtracsysFusionTrack/v_repExtAtracsysFusionTrack.cpp b/src/v_repExtAtracsysFusionTrack/v_repExtAtracsysFusionTrack.cpp index c471df9..4198367 100755 --- a/src/v_repExtAtracsysFusionTrack/v_repExtAtracsysFusionTrack.cpp +++ b/src/v_repExtAtracsysFusionTrack/v_repExtAtracsysFusionTrack.cpp @@ -48,6 +48,11 @@ std::shared_ptr fusionTrackPG; std::shared_ptr loggerPG; /// Recording will begin when the simulation starts running, and log files will be saved every time it stops running. bool recordWhileSimulationIsRunningG = false; +bool shouldPrintConnectionSuccess = true; + +const std::size_t MegaByte = 1024*1024; +// If we write too large a flatbuffer +std::size_t single_buffer_limit_bytes = 20*MegaByte; void removeGeometryID(std::string geometryID_lua_param, grl::FusionTrackLogAndTrack::Params ¶ms) @@ -186,6 +191,7 @@ void LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_OBJECT(SLuaCallBack *p) std::string(" GeometryID: ") + inData->at(3).stringData[0]; // GeometryID loggerPG->info(output); // convert the tuple of strings to a tuple of ints + // convert vrep string names to vrep handle integer ids auto newObjectToTrackIDs = grl::VrepStringToLogAndTrackMotionConfigParams(newObjectToTrack); fusionTrackParamsG.MotionConfigParamsVector.push_back(newObjectToTrackIDs); @@ -283,6 +289,7 @@ void LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_STOP(SLuaCallBack *p) { loggerPG->info("Ending Atracsys Fusion Track Plugin connection to Optical Tracker\n"); fusionTrackPG.reset(); + shouldPrintConnectionSuccess = true; } ///////////////////////////////////////////////////////////////////////////////////////// @@ -293,6 +300,7 @@ void LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_RESET(SLuaCallBack *p) { fusionTrackPG.reset(); LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_START(p); + shouldPrintConnectionSuccess = true; } ///////////////////////////////////////////////////////////////////////////////////////// @@ -315,7 +323,10 @@ void LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_IS_ACTIVE(SLuaCallBack *p) ///////////////////////////////////////////////////////////////////////////////////////// /// LUA function to actually start recording the fusiontrack frame data in memory. ///////////////////////////////////////////////////////////////////////////////////////// - +const int inArgs_LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_START_RECORDING[] = { + 1, + sim_lua_arg_float,0 // Single buffer limit +}; // simExtAtracsysFusionTrackStartRecording void LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_START_RECORDING(SLuaCallBack *p) { @@ -323,10 +334,12 @@ void LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_START_RECORDING(SLuaCallBack *p) bool success = false; if (fusionTrackPG) { - std::string log_message("Starting the recording of fusiontrack frame data in memory.\n"); + std::string log_message("Starting in-memory recording of fusiontrack frame data.\n"); + std::vector *inData = D.getInDataPtr(); simAddStatusbarMessage(log_message.c_str()); loggerPG->info(log_message); - success = fusionTrackPG->start_recording(); + single_buffer_limit_bytes = inData->at(0).floatData[0]; + success = fusionTrackPG->start_recording(single_buffer_limit_bytes); } D.pushOutData(CLuaFunctionDataItem(success)); D.writeDataToLua(p); @@ -341,7 +354,7 @@ void LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_STOP_RECORDING(SLuaCallBack *p) bool success = false; if (fusionTrackPG) { - std::string log_message("Stopping the recording of fusiontrack frame data in memory.\n"); + std::string log_message("Stoping in-memory recording of fusiontrack frame data.\n"); simAddStatusbarMessage(log_message.c_str()); loggerPG->info(log_message); success = fusionTrackPG->stop_recording(); @@ -409,13 +422,16 @@ void LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_SAVE_RECORDING(SLuaCallBack *p) ///////////////////////////////////////////////////////////////////////////// #define LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_RECORD_WHILE_SIMULATION_IS_RUNNING_COMMAND "simExtAtracsysFusionTrackRecordWhileSimulationIsRunning" - +// 2GB is the hard limit for flatbuffer binary file, so when it hits this hard limit, it should write the data from memory to disk. +// Here to make it convenient to analysis the data, you can set it any capacity based on your purpose. +// When it reaches the capacity, new file will be created. const int inArgs_LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_RECORD_WHILE_SIMULATION_IS_RUNNING[] = { - 1, - sim_lua_arg_bool, 1 // string file name + 2, + sim_lua_arg_bool, 1, // string file name + sim_lua_arg_float,0 // Single buffer limit }; -std::string LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_RECORD_WHILE_SIMULATION_IS_RUNNING_CALL_TIP("number result=simExtAtracsysFusionTrackRecordWhileSimulationIsRunning(bool recording)"); +std::string LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_RECORD_WHILE_SIMULATION_IS_RUNNING_CALL_TIP("number result=simExtAtracsysFusionTrackRecordWhileSimulationIsRunning(bool recording, float single_buffer_limit_bytes)"); void LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_RECORD_WHILE_SIMULATION_IS_RUNNING(SLuaCallBack *p) { @@ -429,10 +445,15 @@ void LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_RECORD_WHILE_SIMULATION_IS_RUNNING(SLuaCa { std::vector *inData = D.getInDataPtr(); recordWhileSimulationIsRunningG = inData->at(0).boolData[0]; - - std::string log_message = std::string("simExtAtracsysFusionTrackRecordWhileSimulationIsRunning: ") + boost::lexical_cast(recordWhileSimulationIsRunningG); - simAddStatusbarMessage(log_message.c_str()); - loggerPG->info(log_message); + single_buffer_limit_bytes = inData->at(1).floatData[0]; + // std::cout << "Start recording while simulation is running for Tracker..." << recordWhileSimulationIsRunningG << std::endl; + if (fusionTrackPG && recordWhileSimulationIsRunningG) + { + std::string log_message = std::string("simExtAtracsysFusionTrackRecordWhileSimulationIsRunning: ") + boost::lexical_cast(recordWhileSimulationIsRunningG); + simAddStatusbarMessage(log_message.c_str()); + loggerPG->info(log_message); + success = fusionTrackPG->start_recording(single_buffer_limit_bytes); + } D.pushOutData(CLuaFunctionDataItem(success)); D.writeDataToLua(p); @@ -541,10 +562,14 @@ VREP_DLLEXPORT unsigned char v_repStart(void *reservedPointer, int reservedInt) simRegisterCustomLuaFunction("simExtAtracsysFusionTrackClearObjects", "number result=simExtAtracsysFusionTrackClearObjects()", inArgs1, LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_CLEAR_OBJECTS); /// Register functions to control the recording procedure of the fusiontrack frame data in memory - simRegisterCustomLuaFunction("simExtAtracsysFusionTrackStartRecording", "number result=simExtAtracsysFusionTrackStartRecording()", inArgs1, LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_START_RECORDING); simRegisterCustomLuaFunction("simExtAtracsysFusionTrackStopRecording", "number result=simExtAtracsysFusionTrackStopRecording()", inArgs1, LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_STOP_RECORDING); simRegisterCustomLuaFunction("simExtAtracsysFusionTrackClearRecording", "number result=simExtAtracsysFusionTrackClearRecording()", inArgs1, LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_CLEAR_RECORDING); - + + CLuaFunctionData::getInputDataForFunctionRegistration(inArgs_LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_START_RECORDING, inArgs); + simRegisterCustomLuaFunction("simExtAtracsysFusionTrackStartRecording", + "number result=simExtAtracsysFusionTrackStartRecording()", + &inArgs[0], + LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_START_RECORDING); CLuaFunctionData::getInputDataForFunctionRegistration(inArgs_LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_SAVE_RECORDING, inArgs); simRegisterCustomLuaFunction(LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_SAVE_RECORDING_COMMAND, LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_SAVE_RECORDING_CALL_TIP.c_str(), @@ -660,6 +685,15 @@ VREP_DLLEXPORT void *v_repMessage(int message, int *auxiliaryData, void *customD frameInWhichToMoveObject, pose); } + + // Let the user know when the connection starts successfully and sees a marker :-) + if(shouldPrintConnectionSuccess && transforms.size() > 0) + { + std::string msg("v_repExtAtracsysFusionTrack plugin connected successfully and detected " + std::to_string(transforms.size()) +" markers!"); + simAddStatusbarMessage(msg.c_str()); + loggerPG->info(msg); + shouldPrintConnectionSuccess = false; + } } else if(fusionTrackPG && !fusionTrackPG->is_active()) { @@ -681,7 +715,7 @@ VREP_DLLEXPORT void *v_repMessage(int message, int *auxiliaryData, void *customD } if (message == sim_message_eventcallback_simulationabouttostart) - { // Simulation is about to start + { // Simulation is about to start. CLick the start button in VREP ///////////////////////// // PUT OBJECT STARTUP CODE HERE @@ -704,10 +738,10 @@ VREP_DLLEXPORT void *v_repMessage(int message, int *auxiliaryData, void *customD // } if(fusionTrackPG && recordWhileSimulationIsRunningG) { - fusionTrackPG->start_recording(); + fusionTrackPG->start_recording(single_buffer_limit_bytes); } } - + // simulation has ended if (message == sim_message_eventcallback_simulationended) { // Simulation just ended @@ -715,8 +749,15 @@ VREP_DLLEXPORT void *v_repMessage(int message, int *auxiliaryData, void *customD // SIMULATION STOPS RUNNING HERE // close out as necessary //////////////////// + loggerPG->error("Ending Fusion Tracker plugin connection to Kuka iiwa\n" ); + loggerPG->info(" Atracsys recordWhileSimulationIsRunningG: {}", recordWhileSimulationIsRunningG); + // loggerPG->info("fusionTrackPG->is_recording(): {}",fusionTrackPG->is_recording()); + if(fusionTrackPG && recordWhileSimulationIsRunningG && fusionTrackPG->is_recording()) { - fusionTrackPG->save_recording(); + bool success = fusionTrackPG->save_recording(); + if(success) { + loggerPG->info("Vrep quits successfully in FT..." ); + } fusionTrackPG->stop_recording(); } } diff --git a/src/v_repExtGrlCisstInverseKinematics/CMakeLists.txt b/src/v_repExtGrlCisstInverseKinematics/CMakeLists.txt index c72394b..2ae5470 100644 --- a/src/v_repExtGrlCisstInverseKinematics/CMakeLists.txt +++ b/src/v_repExtGrlCisstInverseKinematics/CMakeLists.txt @@ -32,20 +32,21 @@ #TODO: add cisst-saw found flag and make it work correctly for independent grl https://github.com/jhu-cisst/cisst-saw if(cisst_cisstNumerical_FOUND AND CISSTNETLIB_FOUND AND sawConstraintController_FOUND AND Boost_FOUND) - #TODO: #WARNING: INSTALLER WILL BE BROKEN UNLESS THIS IS CHANGED TO USE basis_target_link_libraries, etc. #@see https://github.com/schuhschuh/cmake-basis/issues/442 basis_include_directories( - ${PROJECT_INCLUDE_DIR}/thirdparty/vrep/include - ${CISSTNETLIB_INCLUDE_DIRS} - ${CISST_INCLUDE_DIR} + ${PROJECT_INCLUDE_DIR}/thirdparty/vrep/include + ${CISSTNETLIB_INCLUDE_DIRS} + ${CISST_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIR} ${sawConstraintController_INCLUDE_DIR} + ${FLATBUFFERS_INCLUDE_DIRS} ## remove it after calibration debug ) # ${CISST_SAW_INCLUDE_DIRS} basis_add_library(v_repExtGrlCisstInverseKinematics SHARED v_repExtGrlInverseKinematics.cpp ) - basis_target_link_libraries(v_repExtGrlCisstInverseKinematics - ${Boost_LIBRARIES} + basis_target_link_libraries(v_repExtGrlCisstInverseKinematics + ${Boost_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} + ${Boost_REGEX_LIBRARY} # ${cisstNetlib_LIBRARY_cisstNetlib} # ${cisstNetlib_LIBRARY_cisstNetlib_gcc} # ${cisstNetlib_LIBRARY_cisstNetlib_blas} @@ -57,6 +58,16 @@ if(cisst_cisstNumerical_FOUND AND CISSTNETLIB_FOUND AND sawConstraintController_ #${CISSTNETLIB_LIBRARIES} ${sawConstraintController_LIBRARIES} sawConstraintController + ${Nanopb_LIBRARIES} + ${CMAKE_THREAD_LIBS_INIT} + ${FLATBUFFERS_STATIC_LIB} + ${LINUX_ONLY_LIBS} + KukaFRIClient + mc_rbdyn_urdf + SpaceVecAlg #TODO: re-enable when exported targets are correct again + sch-core + RBDyn + Tasks ## remove it after calibration work v_repLib ) endif() diff --git a/src/v_repExtGrlInverseKinematics/CMakeLists.txt b/src/v_repExtGrlInverseKinematics/CMakeLists.txt index 7870e9c..c3beffa 100644 --- a/src/v_repExtGrlInverseKinematics/CMakeLists.txt +++ b/src/v_repExtGrlInverseKinematics/CMakeLists.txt @@ -40,26 +40,34 @@ if( SpaceVecAlg_FOUND AND sch-core_FOUND AND RBDyn_FOUND AND Tasks_FOUND AND Boo message(STATUS v_repExtGrlInverseKinematics BUILDING!!!) # TODO(ahundt) link_directories is a hack, remove me! link_directories(/usr/local/lib) - #TODO: #WARNING: INSTALLER WILL BE BROKEN UNLESS THIS IS CHANGED TO USE basis_target_link_libraries, etc. + #TODO: #WARNING: INSTALLER WILL BE BROKEN UNLESS THIS IS CHANGED TO USE basis_target_link_libraries, etc. #@see https://github.com/schuhschuh/cmake-basis/issues/442 basis_include_directories( - ${PROJECT_INCLUDE_DIR}/thirdparty/vrep/include + ${PROJECT_INCLUDE_DIR}/thirdparty/vrep/include ${EIGEN3_INCLUDE_DIR} ${SpaceVecAlg_INCLUDE_DIR} + ${FLATBUFFERS_INCLUDE_DIRS} # TODO(ahundt): include directories from Tasks & its deps #$ #$ # /include/mylib ) # ${CISST_SAW_INCLUDE_DIRS} basis_add_library(v_repExtGrlInverseKinematics SHARED v_repExtGrlInverseKinematics.cpp ) - basis_target_link_libraries(v_repExtGrlInverseKinematics - ${Boost_LIBRARIES} - ${CMAKE_THREAD_LIBS_INIT} - SpaceVecAlg #TODO: re-enable when exported targets are correct again - sch-core - RBDyn - Tasks - v_repLib + basis_target_link_libraries(v_repExtGrlInverseKinematics + ${Boost_LIBRARIES} + ${Boost_REGEX_LIBRARY} + ${FRI_Client_SDK_Cpp_LIBRARIES} + ${Nanopb_LIBRARIES} + ${CMAKE_THREAD_LIBS_INIT} + ${FLATBUFFERS_STATIC_LIB} + ${LINUX_ONLY_LIBS} + KukaFRIClient + mc_rbdyn_urdf + SpaceVecAlg #TODO: re-enable when exported targets are correct again + sch-core + RBDyn + Tasks + v_repLib ) endif() diff --git a/src/v_repExtGrlInverseKinematics/v_repExtGrlInverseKinematics.cpp b/src/v_repExtGrlInverseKinematics/v_repExtGrlInverseKinematics.cpp index 4e8fc56..b2dc8d0 100755 --- a/src/v_repExtGrlInverseKinematics/v_repExtGrlInverseKinematics.cpp +++ b/src/v_repExtGrlInverseKinematics/v_repExtGrlInverseKinematics.cpp @@ -18,7 +18,7 @@ #include #include "v_repExtGrlInverseKinematics.h" #include "grl/vrep/InverseKinematicsVrepPlugin.hpp" - +#include "luaFunctionData.h" #include #include @@ -43,6 +43,9 @@ LIBRARY vrepLib; // the V-REP library that we will dynamically load and bind #define CONCAT(x,y,z) x y z #define strConCat(x,y,z) CONCAT(x,y,z) #define LUA_GET_SENSOR_DATA_COMMAND "simExtSkeleton_getSensorData" +#define LUA_SIM_EXT_GRL_IK_START_COMMAND "simExtGrlInverseKinematicsStart" + + @@ -60,15 +63,27 @@ std::shared_ptr loggerPG; return (p,o); } */ - +const int inArgs_SIM_EXT_GRL_IK_START[]={ + 2, + sim_lua_arg_int, 0, + sim_lua_arg_bool, 0 +}; +std::string LUA_SIM_EXT_GRL_IK_START_CALL_TIP("number result=simExtGrlInverseKinematicsStart(int run_mode, bool commanddataing)"); void LUA_SIM_EXT_GRL_IK_START(SLuaCallBack* p) { - if (!InverseKinematicsControllerPG) { - - loggerPG->error("v_repExtInverseKinematicsController Starting Inverse Kinematics Plugin\n"); - InverseKinematicsControllerPG=std::make_shared(); - InverseKinematicsControllerPG->construct(); - } + if (!InverseKinematicsControllerPG) { + loggerPG->info("v_repExtInverseKinematicsController Starting Inverse Kinematics Plugin\n"); + CLuaFunctionData data; + if (data.readDataFromLua(p, inArgs_SIM_EXT_GRL_IK_START, inArgs_SIM_EXT_GRL_IK_START[0], LUA_SIM_EXT_GRL_IK_START_COMMAND)) + { + std::vector* inData = data.getInDataPtr(); + int run_mode = inData->at(0).intData[0]; + bool commanddata = inData->at(1).boolData[0]; + InverseKinematicsControllerPG = std::make_shared(); + InverseKinematicsControllerPG->construct(); + InverseKinematicsControllerPG->setInvRunMode(run_mode, commanddata); + } + } } void LUA_SIM_EXT_GRL_IK_RESET(SLuaCallBack* p) @@ -179,7 +194,7 @@ VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt) int noArgs[]={0}; // no input arguments - simRegisterCustomLuaFunction("simExtGrlInverseKinematicsStart","number result=simExtGrlInverseKinematicsStart()",noArgs,LUA_SIM_EXT_GRL_IK_START); + // simRegisterCustomLuaFunction("simExtGrlInverseKinematicsStart","number result=simExtGrlInverseKinematicsStart()",noArgs,LUA_SIM_EXT_GRL_IK_START); simRegisterCustomLuaFunction("simExtGrlInverseKinematicsStop","number result=simExtGrlInverseKinematicsStop()",noArgs,LUA_SIM_EXT_GRL_IK_STOP); simRegisterCustomLuaFunction("simExtGrlInverseKinematicsReset","number result=simExtGrlInverseKinematicsReset()",noArgs,LUA_SIM_EXT_GRL_IK_RESET); simRegisterCustomLuaFunction("simExtGrlInverseKinematicsAddFrame","number result=simExtGrlInverseKinematicsAddFrame()",noArgs,LUA_SIM_EXT_GRL_IK_ADD_FRAME); @@ -187,7 +202,16 @@ VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt) simRegisterCustomLuaFunction("simExtGrlInverseKinematicsApplyTransform","number result=simExtGrlInverseKinematicsApplyTransform()",noArgs,LUA_SIM_EXT_GRL_IK_APPLY_TRANSFORM); simRegisterCustomLuaFunction("simExtGrlInverseKinematicsRestoreSensorPosition","number result=simExtGrlInverseKinematicsRestoreSensorPosition()",noArgs,LUA_SIM_EXT_GRL_IK_RESTORE_SENSOR_POSITION); + std::vector inArgs; + CLuaFunctionData::getInputDataForFunctionRegistration(inArgs_SIM_EXT_GRL_IK_START, inArgs); + simRegisterCustomLuaFunction + ( + LUA_SIM_EXT_GRL_IK_START_COMMAND, + LUA_SIM_EXT_GRL_IK_START_CALL_TIP.c_str(), + &inArgs[0], + LUA_SIM_EXT_GRL_IK_START + ); // ****************************************** loggerPG->info("Inverse Kinematics plugin initialized. Build date/time: {} {}", __DATE__, __TIME__); diff --git a/src/v_repExtHandEyeCalibration/camodocal/HandEyeCalibration.cpp b/src/v_repExtHandEyeCalibration/camodocal/HandEyeCalibration.cpp index 52701ef..a1fc5ce 100644 --- a/src/v_repExtHandEyeCalibration/camodocal/HandEyeCalibration.cpp +++ b/src/v_repExtHandEyeCalibration/camodocal/HandEyeCalibration.cpp @@ -140,7 +140,6 @@ HandEyeCalibration::estimateHandEyeScrew(const std::vector(i * 6, 0) = AxisAngleToSTransposeBlockOfT(rvec1,tvec1,rvec2,tvec2); } diff --git a/src/v_repExtHandEyeCalibration/v_repExtHandEyeCalibration.cpp b/src/v_repExtHandEyeCalibration/v_repExtHandEyeCalibration.cpp index ee677aa..34d0030 100755 --- a/src/v_repExtHandEyeCalibration/v_repExtHandEyeCalibration.cpp +++ b/src/v_repExtHandEyeCalibration/v_repExtHandEyeCalibration.cpp @@ -40,6 +40,8 @@ LIBRARY vrepLib; // the V-REP library that we will dynamically load and bind #define CONCAT(x,y,z) x y z #define strConCat(x,y,z) CONCAT(x,y,z) #define LUA_GET_SENSOR_DATA_COMMAND "simExtSkeleton_getSensorData" +#define LUA_SIM_EXT_HAND_EYE_CALIB_START_COMMAND "simExtHandEyeCalibStart" +#define LUA_SIM_EXT_HAND_EYE_CALIB_APPLY_TRANSFORM_COMMAND "simExtHandEyeCalibApplyTransform" @@ -67,7 +69,13 @@ const int inArgs_HAND_EYE_CALIB_START[]={ sim_lua_arg_string,0, // "tcp://0.0.0.0:30010" , // OpticalTrackerTipName }; -std::string LUA_SIM_EXT_HAND_EYE_CALIB_START_CALL_TIP("number result=simExtHandEyeCalibStart(string RobotBaseName , string RobotTipName, string OpticalTrackerBaseName, string OpticalTrackerDetectedObjectName) -- KukaCommandMode options are JAVA and FRI"); +const int inXArgs_HAND_EYE_CALIB_SCENE_NAME[]={ + 1, // Example Value // Parameter name + sim_lua_arg_string,0, // "RobotMillTip" , // RobotBaseName, +}; +// -- KukaCommandMode options are JAVA and FRI +std::string LUA_SIM_EXT_HAND_EYE_CALIB_START_CALL_TIP("number result=simExtHandEyeCalibStart(string RobotBaseName , string RobotTipName, string OpticalTrackerBaseName, string OpticalTrackerDetectedObjectName)"); +std::string LUA_SIM_EXT_HAND_EYE_CALIB_APPLY_TRANSFORM_CALL_TIP("number result=simExtHandEyeCalibApplyTransform(string sceneName)"); void LUA_SIM_EXT_HAND_EYE_CALIB_START(SLuaCallBack* p) @@ -78,7 +86,7 @@ void LUA_SIM_EXT_HAND_EYE_CALIB_START(SLuaCallBack* p) CLuaFunctionData data; - if (data.readDataFromLua(p,inArgs_HAND_EYE_CALIB_START,inArgs_HAND_EYE_CALIB_START[0],"simExtHandEyeCalibStart")) + if (data.readDataFromLua(p,inArgs_HAND_EYE_CALIB_START,inArgs_HAND_EYE_CALIB_START[0], LUA_SIM_EXT_HAND_EYE_CALIB_START_COMMAND)) { std::vector* inData=data.getInDataPtr(); std::string RobotBaseName((inData->at(0 ).stringData[0])); @@ -86,7 +94,7 @@ void LUA_SIM_EXT_HAND_EYE_CALIB_START(SLuaCallBack* p) std::string OpticalTrackerBaseName((inData->at(2 ).stringData[0])); std::string OpticalTrackerDetectedObjectName(inData->at(3 ).stringData[0]); handEyeCalibrationPG=std::make_shared( - std::make_tuple(RobotBaseName , RobotTipName, OpticalTrackerBaseName, OpticalTrackerDetectedObjectName) + // std::make_tuple(RobotBaseName , RobotTipName, OpticalTrackerBaseName, OpticalTrackerDetectedObjectName) ); handEyeCalibrationPG->construct(); @@ -132,7 +140,16 @@ void LUA_SIM_EXT_HAND_EYE_CALIB_FIND_TRANSFORM(SLuaCallBack* p) void LUA_SIM_EXT_HAND_EYE_CALIB_APPLY_TRANSFORM(SLuaCallBack* p) { if (handEyeCalibrationPG) { - handEyeCalibrationPG->applyEstimate(); + CLuaFunctionData data; + + if (data.readDataFromLua(p,inXArgs_HAND_EYE_CALIB_SCENE_NAME,inXArgs_HAND_EYE_CALIB_SCENE_NAME[0], LUA_SIM_EXT_HAND_EYE_CALIB_APPLY_TRANSFORM_COMMAND)) + { + std::vector* inData=data.getInDataPtr(); + std::string sceneName((inData->at(0 ).stringData[0])); + handEyeCalibrationPG->applyEstimate(sceneName); + } + + } } @@ -205,14 +222,34 @@ VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt) } // ****************************************** + std::vector inArgs; + + CLuaFunctionData::getInputDataForFunctionRegistration(inArgs_HAND_EYE_CALIB_START,inArgs); + simRegisterCustomLuaFunction + ( + LUA_SIM_EXT_HAND_EYE_CALIB_START_COMMAND, + LUA_SIM_EXT_HAND_EYE_CALIB_START_CALL_TIP.c_str(), + &inArgs[0], + LUA_SIM_EXT_HAND_EYE_CALIB_START + ); + + CLuaFunctionData::getInputDataForFunctionRegistration(inXArgs_HAND_EYE_CALIB_SCENE_NAME,inArgs); + simRegisterCustomLuaFunction + ( + LUA_SIM_EXT_HAND_EYE_CALIB_APPLY_TRANSFORM_COMMAND, + LUA_SIM_EXT_HAND_EYE_CALIB_APPLY_TRANSFORM_CALL_TIP.c_str(), + &inArgs[0], + LUA_SIM_EXT_HAND_EYE_CALIB_APPLY_TRANSFORM + ); + int noArgs[]={0}; // no input arguments - simRegisterCustomLuaFunction("simExtHandEyeCalibStart","number result=simExtHandEyeCalibStart()",noArgs,LUA_SIM_EXT_HAND_EYE_CALIB_START); + //simRegisterCustomLuaFunction("simExtHandEyeCalibStart","number result=simExtHandEyeCalibStart()",noArgs,LUA_SIM_EXT_HAND_EYE_CALIB_START); + //simRegisterCustomLuaFunction("simExtHandEyeCalibStart","number result=simExtHandEyeCalibStart()",inArgs_HAND_EYE_CALIB_START,LUA_SIM_EXT_HAND_EYE_CALIB_START); simRegisterCustomLuaFunction("simExtHandEyeCalibStop","number result=simExtHandEyeCalibStop()",noArgs,LUA_SIM_EXT_HAND_EYE_CALIB_STOP); simRegisterCustomLuaFunction("simExtHandEyeCalibReset","number result=simExtHandEyeCalibReset()",noArgs,LUA_SIM_EXT_HAND_EYE_CALIB_RESET); simRegisterCustomLuaFunction("simExtHandEyeCalibAddFrame","number result=simExtHandEyeCalibAddFrame()",noArgs,LUA_SIM_EXT_HAND_EYE_CALIB_ADD_FRAME); simRegisterCustomLuaFunction("simExtHandEyeCalibFindTransform","number result=simExtHandEyeCalibFindTransform()",noArgs,LUA_SIM_EXT_HAND_EYE_CALIB_FIND_TRANSFORM); - simRegisterCustomLuaFunction("simExtHandEyeCalibApplyTransform","number result=simExtHandEyeCalibApplyTransform()",noArgs,LUA_SIM_EXT_HAND_EYE_CALIB_APPLY_TRANSFORM); simRegisterCustomLuaFunction("simExtHandEyeCalibRestoreSensorPosition","number result=simExtHandEyeCalibRestoreSensorPosition()",noArgs,LUA_SIM_EXT_HAND_EYE_CALIB_RESTORE_SENSOR_POSITION); diff --git a/src/v_repExtKukaLBRiiwaPlugin/CMakeLists.txt b/src/v_repExtKukaLBRiiwaPlugin/CMakeLists.txt index 72295bd..bc4c6e9 100644 --- a/src/v_repExtKukaLBRiiwaPlugin/CMakeLists.txt +++ b/src/v_repExtKukaLBRiiwaPlugin/CMakeLists.txt @@ -35,8 +35,8 @@ if(TARGET KukaFRIClient AND Boost_FOUND AND Boost_CHRONO_FOUND) ${FRI-Client-SDK_Cpp_INCLUDE_DIRS} ${PROJECT_INCLUDE_DIR}/thirdparty/vrep/include ) - basis_add_library(v_repExtKukaLBRiiwa SHARED - v_repExtKukaLBRiiwa.cpp + basis_add_library(v_repExtKukaLBRiiwa SHARED + v_repExtKukaLBRiiwa.cpp ../../include/grl/kuka/KukaDriver.hpp ../../include/grl/kuka/KukaFRIalgorithm.hpp ../../include/grl/kuka/KukaFRIdriver.hpp @@ -45,13 +45,14 @@ if(TARGET KukaFRIClient AND Boost_FOUND AND Boost_CHRONO_FOUND) ../../include/grl/kuka/KukaJAVAdriver.hpp ../../include/grl/kuka/KukaNanopb.hpp ) - basis_target_link_libraries(v_repExtKukaLBRiiwa + basis_target_link_libraries(v_repExtKukaLBRiiwa PUBLIC - ${Boost_LIBRARIES} - ${Boost_REGEX_LIBRARY} - ${Boost_CHRONO_LIBRARY} - ${CMAKE_THREAD_LIBS_INIT} + ${Boost_LIBRARIES} + ${Boost_REGEX_LIBRARY} + ${Boost_CHRONO_LIBRARY} + ${CMAKE_THREAD_LIBS_INIT} ${Nanopb_LIBRARIES} + ${FLATBUFFERS_LIBRARIES} v_repLib KukaFRIClient ) diff --git a/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp b/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp index fd45653..5027efa 100755 --- a/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp +++ b/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp @@ -42,9 +42,14 @@ LIBRARY vrepLib; // the V-REP library that we will dynamically load and bind #define LUA_KUKA_LBR_IIWA_START_COMMAND "simExtKukaLBRiiwaStart" - -std::shared_ptr kukaPluginPG; +/// PG means pointer global +std::shared_ptr kukaVrepPluginPG; std::shared_ptr loggerPG; +/// Recording will begin when the simulation starts running, and log files will be saved every time it stops running. +bool recordWhileSimulationIsRunningG = false; +const std::size_t MegaByte = 1024*1024; +// If we write too large a flatbuffer +std::size_t single_buffer_limit_bytes = 20*MegaByte; const int inArgs_KUKA_LBR_IIWA_START[]={ 16, // Example Value // Parameter name @@ -63,7 +68,7 @@ const int inArgs_KUKA_LBR_IIWA_START[]={ sim_lua_arg_string,0, // "30200" , // RemoteHostKukaKoniUDPPort sim_lua_arg_string,0, // "JAVA" , // KukaCommandMode (options are "JAVA", "FRI") sim_lua_arg_string,0, // "FRI" , // KukaMonitorMode (options are "JAVA", "FRI") - sim_lua_arg_string,0, // "IK_Group1_iiwa" // IKGroupName (VREP built in inverse kinematics group) + sim_lua_arg_string,0 // "IK_Group1_iiwa" // IKGroupName (VREP built in inverse kinematics group) }; std::string LUA_KUKA_LBR_IIWA_START_CALL_TIP("number result=simExtKukaLBRiiwaStart(string_table JointHandles , string RobotTipHandle, string RobotFlangeTipHandle, string RobotTargetHandle, string RobotTargetBaseHandle, string RobotModel, string LocalUDPAddress, string LocalUDPPort, string RemoteUDPAddress, string LocalHostKukaKoniUDPAddress, string LocalHostKukaKoniUDPPort, string RemoteHostKukaKoniUDPAddress, string RemoteHostKukaKoniUDPPort, string KukaCommandMode, string KukaMonitorMode, string IKGroupName) -- KukaCommandMode options are JAVA and FRI"); @@ -73,15 +78,17 @@ void LUA_SIM_EXT_KUKA_LBR_IIWA_START(SLuaCallBack* p) // return Lua Table or arrays containing position, torque, torque minus motor force, timestamp, FRI state try { - if (!kukaPluginPG) { - loggerPG->error("Starting KUKA LBR iiwa plugin connection to Kuka iiwa\n" ); + /// + if (!kukaVrepPluginPG) { + loggerPG->error("simExtKukaLBRiiwaStart: Starting KUKA LBR iiwa plugin connection to Kuka iiwa...\n kukaVrepPluginPG hasn't been initialized...\n" ); + } CLuaFunctionData data; if (data.readDataFromLua(p,inArgs_KUKA_LBR_IIWA_START,inArgs_KUKA_LBR_IIWA_START[0],LUA_KUKA_LBR_IIWA_START_COMMAND)) { - std::vector* inData=data.getInDataPtr(); + std::vector* inData = data.getInDataPtr(); std::vector JointHandles; for (size_t i=0;iat(0).stringData.size();i++) { @@ -103,8 +110,7 @@ void LUA_SIM_EXT_KUKA_LBR_IIWA_START(SLuaCallBack* p) std::string KukaMonitorMode (inData->at(14).stringData[0]); std::string IKGroupName (inData->at(15).stringData[0]); - - kukaPluginPG=std::make_shared( + kukaVrepPluginPG = std::make_shared( std::make_tuple( JointHandles , RobotFlangeTipHandle , @@ -124,41 +130,169 @@ void LUA_SIM_EXT_KUKA_LBR_IIWA_START(SLuaCallBack* p) IKGroupName ) ); - kukaPluginPG->construct(); + kukaVrepPluginPG->construct(); } else { /// @todo report an error? // use default params - kukaPluginPG=std::make_shared(); - kukaPluginPG->construct(); + kukaVrepPluginPG=std::make_shared(); + kukaVrepPluginPG->construct(); } - - - - } - } catch (const boost::exception& e){ // log the error and print it to the screen, don't release the exception std::string initerr("v_repExtKukaLBRiiwa plugin encountered the following error and will disable itself:\n" + boost::diagnostic_information(e)); simAddStatusbarMessage( initerr.c_str()); loggerPG->error( initerr ); - kukaPluginPG.reset(); + kukaVrepPluginPG.reset(); } catch (const std::exception& e){ // log the error and print it to the screen, don't release the exception std::string initerr("v_repExtKukaLBRiiwa plugin encountered the following error and will disable itself:\n" + boost::diagnostic_information(e)); simAddStatusbarMessage( initerr.c_str()); loggerPG->error( initerr ); - kukaPluginPG.reset(); + kukaVrepPluginPG.reset(); } catch (...){ // log the error and print it to the screen, don't release the exception std::string initerr("v_repExtKukaLBRiiwa plugin encountered an unknown error and will disable itself. Please debug this issue! file and line:" + std::string(__FILE__) + " " + boost::lexical_cast(__LINE__) + "\n"); simAddStatusbarMessage( initerr.c_str()); loggerPG->error( initerr ); - kukaPluginPG.reset(); + kukaVrepPluginPG.reset(); } } +///////////////////////////////////////////////////////////////////////////////////////// +/// LUA function to actually start recording the fusiontrack frame data in memory. +///////////////////////////////////////////////////////////////////////////////////////// +const int inArgs_LUA_SIM_EXT_KUKA_LBR_IIWA_START_RECORDING[] = { + 1, + sim_lua_arg_float,0 // Single buffer limit +}; +// simExtKUKAiiwaStartRecording +void LUA_SIM_EXT_KUKA_LBR_IIWA_START_RECORDING(SLuaCallBack *p) +{ + CLuaFunctionData D; + bool success = false; + if (kukaVrepPluginPG) + { + std::string log_message("Starting the recording of KUKAiiwa state data in memory.\n"); + std::vector *inData = D.getInDataPtr(); + simAddStatusbarMessage(log_message.c_str()); + loggerPG->info(log_message); + single_buffer_limit_bytes = inData->at(0).floatData[0]; + success = kukaVrepPluginPG->start_recording(single_buffer_limit_bytes); + } + D.pushOutData(CLuaFunctionDataItem(success)); + D.writeDataToLua(p); +} +// simExtKUKAiiwaStopRecording +void LUA_SIM_EXT_KUKA_LBR_IIWA_STOP_RECORDING(SLuaCallBack *p) +{ + CLuaFunctionData D; + bool success = false; + if (kukaVrepPluginPG) + { + std::string log_message("Stoping the recording of KUKAiiwa state data in memory.\n"); + simAddStatusbarMessage(log_message.c_str()); + loggerPG->info(log_message); + success = kukaVrepPluginPG->stop_recording(); + } + D.pushOutData(CLuaFunctionDataItem(success)); + D.writeDataToLua(p); +} + +///////////////////////////////////////////////////////////////////////////// +// Save the currently recorded fusiontrack frame data, this also clears the recording. +///////////////////////////////////////////////////////////////////////////// +// simExtKUKAiiwaStartRecording +#define LUA_SIM_EXT_KUKA_LBR_IIWA_SAVE_RECORDING_COMMAND "simExtKukaLBRiiwaSaveRecording" + +const int inArgs_LUA_SIM_EXT_KUKA_LBR_IIWA_SAVE_RECORDING[] = { + 1, + sim_lua_arg_string, 0 // string file name +}; + +std::string LUA_SIM_EXT_KUKA_LBR_IIWA_SAVE_RECORDING_CALL_TIP("number result=simExtKukaLBRiiwaSaveRecording(string filename)"); + +void LUA_SIM_EXT_KUKA_LBR_IIWA_SAVE_RECORDING(SLuaCallBack *p) +{ + CLuaFunctionData D; + bool success = false; + if (D.readDataFromLua(p, inArgs_LUA_SIM_EXT_KUKA_LBR_IIWA_SAVE_RECORDING, inArgs_LUA_SIM_EXT_KUKA_LBR_IIWA_SAVE_RECORDING[0], LUA_SIM_EXT_KUKA_LBR_IIWA_SAVE_RECORDING_COMMAND)) + { + std::vector *inData = D.getInDataPtr(); + std::string filename_lua_param(inData->at(0).stringData[0]); + if (kukaVrepPluginPG) + { + std::string log_message("Saving the recording of KUKAiiwa state data in memory.\n"); + simAddStatusbarMessage(log_message.c_str()); + loggerPG->info(log_message); + success = kukaVrepPluginPG->save_recording(); + } + D.pushOutData(CLuaFunctionDataItem(success)); + D.writeDataToLua(p); + } + + +} + +// simExtKUKAiiwaStartRecording +void LUA_SIM_EXT_KUKA_LBR_IIWA_CLEAR_RECORDING(SLuaCallBack *p) +{ + CLuaFunctionData D; + bool success = false; + if (kukaVrepPluginPG) + { + std::string log_message("Clearing the recording of KUKAiiwa state data in memory.\n"); + simAddStatusbarMessage(log_message.c_str()); + loggerPG->info(log_message); + kukaVrepPluginPG->clear_recording(); + success = true; + } + D.pushOutData(CLuaFunctionDataItem(success)); + D.writeDataToLua(p); +} + +///////////////////////////////////////////////////////////////////////////// +// Set recordWhileSimulationIsRunningG +///////////////////////////////////////////////////////////////////////////// + +#define LUA_SIM_EXT_KUKA_LBR_IIWA_RECORD_WHILE_SIMULATION_IS_RUNNING_COMMAND "simExtKukaLBRiiwaRecordWhileSimulationIsRunning" + +const int inArgs_LUA_SIM_EXT_KUKA_LBR_IIWA_RECORD_WHILE_SIMULATION_IS_RUNNING[] = { + 2, + sim_lua_arg_bool, 1, // string file name + sim_lua_arg_float,0 // Single buffer limit +}; + +std::string LUA_SIM_EXT_KUKA_LBR_IIWA_RECORD_WHILE_SIMULATION_IS_RUNNING_CALL_TIP("number result=simExtKukaLBRiiwaRecordWhileSimulationIsRunning(bool recording, float single_buffer_limit_bytes)"); + +void LUA_SIM_EXT_KUKA_LBR_IIWA_RECORD_WHILE_SIMULATION_IS_RUNNING(SLuaCallBack *p) +{ + + CLuaFunctionData D; + bool success = false; + if (D.readDataFromLua(p, + inArgs_LUA_SIM_EXT_KUKA_LBR_IIWA_RECORD_WHILE_SIMULATION_IS_RUNNING, + inArgs_LUA_SIM_EXT_KUKA_LBR_IIWA_RECORD_WHILE_SIMULATION_IS_RUNNING[0], + LUA_SIM_EXT_KUKA_LBR_IIWA_RECORD_WHILE_SIMULATION_IS_RUNNING_COMMAND)) + { + std::vector *inData = D.getInDataPtr(); + recordWhileSimulationIsRunningG = inData->at(0).boolData[0]; + single_buffer_limit_bytes = inData->at(1).floatData[0]; + std::cout << "Start recording while simulation is running..." << recordWhileSimulationIsRunningG << std::endl; + if (kukaVrepPluginPG && recordWhileSimulationIsRunningG) + { + std::string log_message = std::string("simExtKukaLBRiiwaRecordWhileSimulationIsRunning: ") + boost::lexical_cast(recordWhileSimulationIsRunningG); + simAddStatusbarMessage(log_message.c_str()); + loggerPG->info(log_message); + kukaVrepPluginPG->start_recording(single_buffer_limit_bytes); + } + + D.pushOutData(CLuaFunctionDataItem(success)); + D.writeDataToLua(p); + } +} + // This is the plugin start routine (called just once, just after the plugin was loaded): VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt) { @@ -225,14 +359,28 @@ VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt) LUA_SIM_EXT_KUKA_LBR_IIWA_START ); - - - // Expected input arguments are: int sensorIndex, float floatParameters[3], int intParameters[2] - //int inArgs_getSensorData[]={3,sim_lua_arg_int,sim_lua_arg_float|sim_lua_arg_table,sim_lua_arg_int|sim_lua_arg_table}; // this says we expect 3 arguments (1 integer, a table of floats, and a table of ints) - // Return value can change on the fly, so no need to specify them here, except for the calltip. - // Now register the callback: - //simRegisterCustomLuaFunction(LUA_GET_SENSOR_DATA_COMMAND,strConCat("number result,table data,number distance=",LUA_GET_SENSOR_DATA_COMMAND,"(number sensorIndex,table_3 floatParams,table_2 intParams)"),inArgs_getSensorData,LUA_GET_SENSOR_DATA_CALLBACK); - // ****************************************** + int inArgs1[] = {0}; // no input arguments + /// Register functions to control the recording procedure of the fusiontrack frame data in memory + simRegisterCustomLuaFunction("simExtKukaLBRiiwaStopRecording", "number result=simExtKukaLBRiiwaStopRecording()", inArgs1, LUA_SIM_EXT_KUKA_LBR_IIWA_STOP_RECORDING); + simRegisterCustomLuaFunction("simExtKukaLBRiiwaClearRecording", "number result=simExtKukaLBRiiwaClearRecording()", inArgs1, LUA_SIM_EXT_KUKA_LBR_IIWA_CLEAR_RECORDING); + + CLuaFunctionData::getInputDataForFunctionRegistration(inArgs_LUA_SIM_EXT_KUKA_LBR_IIWA_START_RECORDING, inArgs); + simRegisterCustomLuaFunction("simExtKukaLBRiiwaStartRecording", + "number result=simExtKukaLBRiiwaStartRecording(simxFloat singlebuffersize)", + &inArgs[0], + LUA_SIM_EXT_KUKA_LBR_IIWA_START_RECORDING); + + CLuaFunctionData::getInputDataForFunctionRegistration(inArgs_LUA_SIM_EXT_KUKA_LBR_IIWA_SAVE_RECORDING, inArgs); + simRegisterCustomLuaFunction(LUA_SIM_EXT_KUKA_LBR_IIWA_SAVE_RECORDING_COMMAND, + LUA_SIM_EXT_KUKA_LBR_IIWA_SAVE_RECORDING_CALL_TIP.c_str(), + &inArgs[0], + LUA_SIM_EXT_KUKA_LBR_IIWA_SAVE_RECORDING); + + CLuaFunctionData::getInputDataForFunctionRegistration(inArgs_LUA_SIM_EXT_KUKA_LBR_IIWA_RECORD_WHILE_SIMULATION_IS_RUNNING, inArgs); + simRegisterCustomLuaFunction(LUA_SIM_EXT_KUKA_LBR_IIWA_RECORD_WHILE_SIMULATION_IS_RUNNING_COMMAND, + LUA_SIM_EXT_KUKA_LBR_IIWA_RECORD_WHILE_SIMULATION_IS_RUNNING_CALL_TIP.c_str(), + &inArgs[0], + LUA_SIM_EXT_KUKA_LBR_IIWA_RECORD_WHILE_SIMULATION_IS_RUNNING); loggerPG->error("KUKA LBR iiwa plugin initialized. Build date/time: ", __DATE__, " ", __TIME__ ); @@ -248,8 +396,7 @@ VREP_DLLEXPORT void v_repEnd() // PUT OBJECT RESET CODE HERE // close out as necessary //////////////////// - - kukaPluginPG.reset(); + kukaVrepPluginPG.reset(); unloadVrepLibrary(vrepLib); // release the library } @@ -294,9 +441,6 @@ VREP_DLLEXPORT void* v_repMessage(int message,int* auxiliaryData,void* customDat { // we actualize plugin objects for changes in the scene refreshDlgFlag=true; // always a good idea to trigger a refresh of this plugin's dialog here } - - - //... ////////////// // PUT MAIN CODE HERE @@ -304,7 +448,7 @@ VREP_DLLEXPORT void* v_repMessage(int message,int* auxiliaryData,void* customDat ///////////// if (simGetSimulationState() != sim_simulation_advancing_abouttostop) //checks if the simulation is still running { - //if(kukaPluginPG) loggerPG->error("current simulation time:" << simGetSimulationTime() << std::endl ); // gets simulation time point + //if(kukaVrepPluginPG) loggerPG->error("current simulation time:" << simGetSimulationTime() << std::endl ); // gets simulation time point } // make sure it is "right" (what does that mean?) @@ -314,31 +458,31 @@ VREP_DLLEXPORT void* v_repMessage(int message,int* auxiliaryData,void* customDat // Use handles that were found at the "start" of this simulation running // next few Lines get the joint angles, torque, etc from the simulation - if (kukaPluginPG)// && kukaPluginPG->allHandlesSet == true // allHandlesSet now handled internally + if (kukaVrepPluginPG)// && kukaVrepPluginPG->allHandlesSet == true // allHandlesSet now handled internally { try { // run one loop synchronizing the arm and plugin - kukaPluginPG->run_one(); + kukaVrepPluginPG->run_one(); } catch (const boost::exception& e){ // log the error and print it to the screen, don't release the exception std::string initerr("v_repExtKukaLBRiiwa plugin encountered the following error and will disable itself:\n" + boost::diagnostic_information(e)); simAddStatusbarMessage( initerr.c_str()); loggerPG->error( initerr ); - kukaPluginPG.reset(); + kukaVrepPluginPG.reset(); } catch (const std::exception& e){ // log the error and print it to the screen, don't release the exception std::string initerr("v_repExtKukaLBRiiwa plugin encountered the following error and will disable itself:\n" + boost::diagnostic_information(e)); simAddStatusbarMessage( initerr.c_str()); loggerPG->error( initerr ); - kukaPluginPG.reset(); + kukaVrepPluginPG.reset(); } catch (...){ // log the error and print it to the screen, don't release the exception std::string initerr("v_repExtKukaLBRiiwa plugin encountered an unknown error and will disable itself. Please debug this issue! file and line:" + std::string(__FILE__) + " " + boost::lexical_cast(__LINE__) + "\n"); simAddStatusbarMessage( initerr.c_str()); loggerPG->error( initerr ); - kukaPluginPG.reset(); + kukaVrepPluginPG.reset(); } } @@ -361,16 +505,19 @@ VREP_DLLEXPORT void* v_repMessage(int message,int* auxiliaryData,void* customDat // try { // loggerPG->error("Starting KUKA LBR iiwa plugin connection to Kuka iiwa\n" ); -// kukaPluginPG = std::make_shared(); -// kukaPluginPG->construct(); -// //kukaPluginPG->run_one(); // for debugging purposes only -// //kukaPluginPG.reset(); // for debugging purposes only +// kukaVrepPluginPG = std::make_shared(); +// kukaVrepPluginPG->construct(); +// //kukaVrepPluginPG->run_one(); // for debugging purposes only +// //kukaVrepPluginPG.reset(); // for debugging purposes only // } catch (boost::exception& e){ // // log the error and print it to the screen, don't release the exception // std::string initerr("v_repExtKukaLBRiiwa plugin initialization error:\n" + boost::diagnostic_information(e)); // simAddStatusbarMessage( initerr.c_str()); // loggerPG->error( initerr ); // } + if(kukaVrepPluginPG && recordWhileSimulationIsRunningG) { + kukaVrepPluginPG->start_recording(single_buffer_limit_bytes); + } } if (message==sim_message_eventcallback_simulationended) @@ -381,7 +528,18 @@ VREP_DLLEXPORT void* v_repMessage(int message,int* auxiliaryData,void* customDat // close out as necessary //////////////////// loggerPG->error("Ending KUKA LBR iiwa plugin connection to Kuka iiwa\n" ); - kukaPluginPG.reset(); + loggerPG->info("recordWhileSimulationIsRunningG: {}", recordWhileSimulationIsRunningG); + if (kukaVrepPluginPG.get() != nullptr) { + + loggerPG->info("kukaVrepPluginPG->is_recording(): {}", kukaVrepPluginPG->is_recording()); + + if(kukaVrepPluginPG && recordWhileSimulationIsRunningG && kukaVrepPluginPG->is_recording()) { + bool success = kukaVrepPluginPG->save_recording(); + kukaVrepPluginPG->stop_recording(); + } + kukaVrepPluginPG->clear_recording(); + kukaVrepPluginPG.reset(); + } } diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 879a800..0e78c6c 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -78,18 +78,46 @@ endif () # For KUKA IIWA FRI Libraries if(TARGET KukaFRIClient OR FRI_Client_SDK_Cpp_FOUND) - basis_include_directories(${FRI_Client_SDK_Cpp_PROJECT_INCLUDE_DIRS} ${Boost_REGEX_LIBRARY} ${FRI_Client_SDK_Cpp_INCLUDE_DIRS} ${PROJECT_INCLUDE_DIR}/thirdparty/vrep/include) + basis_include_directories(${FRI_Client_SDK_Cpp_PROJECT_INCLUDE_DIRS} ${Boost_REGEX_LIBRARY} + ${FRI_Client_SDK_Cpp_INCLUDE_DIRS} ${PROJECT_INCLUDE_DIR}/thirdparty/vrep/include + ${FLATBUFFERS_INCLUDE_DIRS} ${FUSIONTRACK_INCLUDE_DIRS}) basis_add_executable(KukaFRITest.cpp)# ${GRL_FLATBUFFERS_OUTPUTS}) - basis_target_link_libraries(KukaFRITest ${Boost_LIBRARIES} ${Boost_REGEX_LIBRARY} ${FRI_Client_SDK_Cpp_LIBRARIES} ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} KukaFRIClient) + basis_target_link_libraries(KukaFRITest ${Boost_LIBRARIES} ${Boost_REGEX_LIBRARY} ${FRI_Client_SDK_Cpp_LIBRARIES} ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} ${FLATBUFFERS_STATIC_LIB} KukaFRIClient ) if(UNIX AND NOT APPLE) set(LINUX_ONLY_LIBS ${LIBDL_LIBRARIES}) endif() basis_add_test(KukaLBRiiwaVrepPluginTest.cpp) - basis_target_link_libraries(KukaLBRiiwaVrepPluginTest ${Boost_LIBRARIES} ${Boost_REGEX_LIBRARY} ${Boost_UNIT_TEST_FRAMEWORK_LIBRARY} ${FRI_Client_SDK_Cpp_LIBRARIES} ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} - v_repLib KukaFRIClient ${LINUX_ONLY_LIBS} ) + basis_target_link_libraries(KukaLBRiiwaVrepPluginTest + ${Boost_LIBRARIES} + ${Boost_REGEX_LIBRARY} + ${Boost_UNIT_TEST_FRAMEWORK_LIBRARY} + ${FRI_Client_SDK_Cpp_LIBRARIES} + ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} + ${FLATBUFFERS_STATIC_LIB} v_repLib KukaFRIClient ${LINUX_ONLY_LIBS} ) + + basis_add_executable(FlatbuffersExampleWithKukaIiwaTest.cpp)# ${GRL_FLATBUFFERS_OUTPUTS}) + basis_target_link_libraries(FlatbuffersExampleWithKukaIiwaTest ${Boost_LIBRARIES} ${Boost_REGEX_LIBRARY} ${FRI_Client_SDK_Cpp_LIBRARIES} ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} KukaFRIClient ${FLATBUFFERS_STATIC_LIB}) + + basis_add_executable(readFlatbufferTest.cpp) + basis_target_link_libraries(readFlatbufferTest + ${Boost_LIBRARIES} + ${Boost_REGEX_LIBRARY} + ${FRI_Client_SDK_Cpp_LIBRARIES} + ${Nanopb_LIBRARIES} + ${CMAKE_THREAD_LIBS_INIT} + ${FLATBUFFERS_STATIC_LIB} + ${LINUX_ONLY_LIBS} + KukaFRIClient + mc_rbdyn_urdf + SpaceVecAlg #TODO: re-enable when exported targets are correct again + sch-core + RBDyn + Tasks + v_repLib) + endif() diff --git a/test/FlatbuffersExampleWithKukaIiwaTest.cpp b/test/FlatbuffersExampleWithKukaIiwaTest.cpp new file mode 100644 index 0000000..954d4f3 --- /dev/null +++ b/test/FlatbuffersExampleWithKukaIiwaTest.cpp @@ -0,0 +1,422 @@ +#define BOOST_TEST_DYN_LINK +#define BOOST_TEST_MODULE grlTest + +#define FLATBUFFERS_DEBUG_VERIFICATION_FAILUR +// system includes +#include +#include +#include +#include + +#include "grl/flatbuffer/flatbuffer.hpp" +#include "grl/kuka/KukaToFlatbuffer.hpp" +#include "grl/kuka/KukaJAVAdriver.hpp" + +#include "grl/kuka/Kuka.hpp" +#include "grl/kuka/KukaDriver.hpp" + +/// Make reference to void map_repeatedDouble() in pb_frimessages_callback.c and pb_frimessages_callbacks.h +std::shared_ptr makeupJointValues(std::vector &jointvalue) { + /// Make reference to void map_repeatedDouble() in pb_frimessages_callback.c and pb_frimessages_callbacks.h + std::shared_ptr values(std::make_shared()); + int numDOF = 7; + values->funcs.encode = &encode_repeatedDouble; + values->funcs.decode = &decode_repeatedDouble; + tRepeatedDoubleArguments *arg = new tRepeatedDoubleArguments; + // map the local container data to the message data fields + arg->max_size = numDOF; + arg->size = 0; + arg->value = (double*) malloc(numDOF * sizeof(double)); + + for(int i=0; ivalue[i] = jointvalue[i]; + } + values->arg = arg; + return values; +} + +volatile std::sig_atomic_t signalStatusG = 0; + +// called when the user presses ctrl+c +void signal_handler(int signal) +{ + signalStatusG = signal; +} + +int charP_size(const char* buf) +{ + int Size = 0; + while (buf[Size] != '\0') Size++; + return Size; +} + +using boost::asio::ip::udp; + + +enum { max_length = 1024 }; + +enum class HowToMove +{ + remain_stationary, + absolute_position, + relative_position, + absolute_position_with_relative_rotation +}; + +enum class DriverToUse +{ + low_level_fri_function, + low_level_fri_class, + kuka_driver_high_level_class +}; + +BOOST_AUTO_TEST_SUITE(KukaTest) + +BOOST_AUTO_TEST_CASE(runRepeatedly) +{ + const std::size_t MegaByte = 1024*1024; + // In 32bits, the maximum size of a single flatbuffer is 2GB - 1 (defined in base.h), but actually single_buffer_limit_bytes can't be set beyond 2045 MB, otherwise assert might be launched. + const std::size_t single_buffer_limit_bytes = 2045*MegaByte; + // Install a signal handler to catch a signal when CONTROL+C + std::signal(SIGINT, signal_handler); + bool OK = true; + bool debug = true; + if (debug) std::cout << "starting KukaTest" << std::endl; + + + std::string localhost("192.170.10.100"); + std::string localport("30200"); + std::string remotehost("192.170.10.2"); + std::string remoteport("30200"); + // A single class for an I/O service object. + boost::asio::io_service io_service; + std::shared_ptr friData(std::make_shared(7)); + cartographer::common::Time startTime; + BOOST_VERIFY(friData); + std::vector ipoJointPos(7,0); + std::vector jointOffset(7,0); // length 7, value 0 + boost::container::static_vector jointStateToCommand(7,0); + grl::robot::arm::KukaState armState; + std::unique_ptr lowLevelStepAlgorithmP; + + // Need to tell the system how long in milliseconds it has to reach the goal + // or it will never move! + std::size_t goal_position_command_time_duration = 4; + lowLevelStepAlgorithmP.reset(new grl::robot::arm::LinearInterpolation()); + + std::shared_ptr> highLevelDriverClassP; + DriverToUse driverToUse = DriverToUse::kuka_driver_high_level_class; + if(driverToUse == DriverToUse::low_level_fri_class) + { + /// @todo TODO(ahundt) BUG: Need way to supply time to reach specified goal for position control and eliminate this allocation internally in the kuka driver. See similar comment in KukaFRIDriver.hpp + /// IDEA: PASS A LOW LEVEL STEP ALGORITHM PARAMS OBJECT ON EACH UPDATE AND ONLY ONE INSTANCE OF THE ALGORITHM OBJECT ITSELF + highLevelDriverClassP = std::make_shared>(io_service, + std::make_tuple("KUKA_LBR_IIWA_14_R820", + localhost, + localport, + remotehost, + remoteport/*,4 ms per tick*/, + grl::robot::arm::KukaFRIClientDataDriver::run_automatically)); + } + std::shared_ptr socketP; + + std::shared_ptr kukaDriverP; + double duration = boost::chrono::high_resolution_clock::now().time_since_epoch().count(); + + + if(driverToUse == DriverToUse::kuka_driver_high_level_class) + { + grl::robot::arm::KukaDriver::Params params = std::make_tuple( + "Robotiiwa" , // RobotName, + "KUKA_LBR_IIWA_14_R820" , // RobotModel (options are KUKA_LBR_IIWA_14_R820, KUKA_LBR_IIWA_7_R800) + "0.0.0.0" , // LocalUDPAddress + "30010" , // LocalUDPPort + "172.31.1.147" , // RemoteUDPAddress + "192.170.10.100" , // LocalHostKukaKoniUDPAddress, + "30200" , // LocalHostKukaKoniUDPPort, + remotehost , // RemoteHostKukaKoniUDPAddress, + remoteport , // RemoteHostKukaKoniUDPPort + "FRI" , // KukaCommandMode (options are FRI, JAVA) + "FRI" // KukaMonitorMode (options are FRI, JAVA) + ); + /// @todo TODO(ahundt) Currently assumes ip address + kukaDriverP=std::make_shared(params); + // kukaDriverP->construct(); + // Default to joint servo mode for commanding motion + kukaDriverP->set(grl::flatbuffer::ArmState::MoveArmJointServo); + kukaDriverP->set(goal_position_command_time_duration,grl::time_duration_command_tag()); + // std::cout << "KUKA COMMAND MODE: " << std::get(params) << "\n"; + } + + unsigned int num_missed = 0; + + for (std::size_t i = 0; !signalStatusG; ++i) { + + /// perform the update step, receiving and sending data to/from the arm + boost::system::error_code send_ec, recv_ec; + std::size_t send_bytes_transferred = 0, recv_bytes_transferred = 0; + bool haveNewData = false; + grl::TimeEvent time_event_stamp; + if(driverToUse == DriverToUse::kuka_driver_high_level_class) + { + kukaDriverP->set( jointStateToCommand, grl::revolute_joint_angle_open_chain_command_tag()); + kukaDriverP->run_one(); + //kukaDriverP->get(); + } + + /// use the interpolated joint position from the previous update as the base + /// The interpolated position is where the java side is commanding, + /// or the fixed starting position with a hold position command on the java side. + if(i!=0 && friData){ + grl::robot::arm::copy(friData->monitoringMsg,ipoJointPos.begin(),grl::revolute_joint_angle_interpolated_open_chain_state_tag()); + } + + + } + std::shared_ptr fbbP; + fbbP = std::make_shared(); + std::string RobotName("Robotiiwa" ); + std::string destination("where this message is going (URI)"); + std::string source("where this message came from (URI)"); + std::string basename = RobotName; //std::get<0>(params); + bool setArmConfiguration_ = true; // set the arm config first time + bool max_control_force_stop_ = false; + + grl::flatbuffer::ArmState armControlMode = grl::flatbuffer::ArmState::StartArm; + grl::flatbuffer::KUKAiiwaInterface commandInterface = grl::flatbuffer::KUKAiiwaInterface::SmartServo;// KUKAiiwaInterface::SmartServo; + grl::flatbuffer::KUKAiiwaInterface monitorInterface = grl::flatbuffer::KUKAiiwaInterface::FRI; + ::ControlMode controlMode = ::ControlMode::ControlMode_POSITION_CONTROLMODE; + ::ClientCommandMode clientCommandMode = ::ClientCommandMode::ClientCommandMode_POSITION; + ::OverlayType overlayType = ::OverlayType::OverlayType_NO_OVERLAY; + ::ConnectionInfo connectionInfo{::FRISessionState::FRISessionState_IDLE, + ::FRIConnectionQuality::FRIConnectionQuality_POOR, + true, 4, false, 0}; + + + std::vector joint_stiffness_(7, 0.2); + std::vector joint_damping_(7, 0.3); + std::vector joint_AccelerationRel_(7, 0.5); + std::vector joint_VelocityRel_(7, 1.0); + bool updateMinimumTrajectoryExecutionTime = false; + double minimumTrajectoryExecutionTime = 4; + + bool logdata = false; + if (logdata == true) { + int64_t sequenceNumber = 0; + //Cartesian Impedance Values + grl::flatbuffer::Vector3d cart_stiffness_trans_ = grl::flatbuffer::Vector3d(500,500,500); + grl::flatbuffer::EulerRotation cart_stifness_rot_ = grl::flatbuffer::EulerRotation(200,200,200,grl::flatbuffer::EulerOrder::xyz); + grl::flatbuffer::Vector3d cart_damping_trans_ = grl::flatbuffer::Vector3d(0.3,0.3,0.3); + grl::flatbuffer::EulerRotation cart_damping_rot_ = grl::flatbuffer::EulerRotation(0.3,0.3,0.3,grl::flatbuffer::EulerOrder::xyz); + grl::flatbuffer::EulerPose cart_stiffness_ = grl::flatbuffer::EulerPose(cart_stiffness_trans_, cart_stifness_rot_); + grl::flatbuffer::EulerPose cart_damping_ = grl::flatbuffer::EulerPose(cart_damping_trans_, cart_damping_rot_); + + grl::flatbuffer::EulerPose cart_max_path_deviation_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(1000,1000,100), grl::flatbuffer::EulerRotation(5.,5.,5., grl::flatbuffer::EulerOrder::xyz)); + grl::flatbuffer::EulerPose cart_max_ctrl_vel_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(1000,1000,1000), grl::flatbuffer::EulerRotation(6.3,6.3,6.3, grl::flatbuffer::EulerOrder::xyz)); + grl::flatbuffer::EulerPose cart_max_ctrl_force_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(200,200,200), grl::flatbuffer::EulerRotation(200.,200.,200., grl::flatbuffer::EulerOrder::xyz)); + double nullspace_stiffness_ = 0.1; + double nullspace_damping_ = 0.1; + + ::MessageHeader messageHeader{1,2,3}; + ::RobotInfo robotInfo{true, 7, true, ::SafetyState::SafetyState_NORMAL_OPERATION}; + ::JointValues strjointValues; + std::vector jointValues(7, 0.1); + boost::container::static_vector jointstate; + for(int i = 0; i<7; i++) { + jointstate.push_back(jointValues[i]); + } + /// Make reference to void map_repeatedDouble() in pb_frimessages_callback.c and pb_frimessages_callbacks.h + strjointValues.value = *makeupJointValues(jointValues); + grl::robot::arm::KukaState::joint_state kukajointstate(jointstate); + + //////////////////////////////////////////////////////////// + // Double check the initialization of this parameter ////// + ////////////////////////////////////////////////////////// + + armState.position = kukajointstate; + armState.torque = kukajointstate; + armState.externalTorque = kukajointstate; + armState.commandedPosition = kukajointstate; + armState.commandedTorque = kukajointstate; + armState.ipoJointPosition = kukajointstate; + armState.ipoJointPositionOffsets = kukajointstate; + armState.commandedCartesianWrenchFeedForward = kukajointstate; + armState.externalForce = kukajointstate; + armState.sessionState = grl::toFlatBuffer(connectionInfo.sessionState); + armState.connectionQuality = grl::toFlatBuffer(connectionInfo.quality); + armState.safetyState = grl::toFlatBuffer(::SafetyState::SafetyState_NORMAL_OPERATION); + armState.operationMode = grl::toFlatBuffer(OperationMode::OperationMode_TEST_MODE_1); + armState.driveState = grl::toFlatBuffer(::DriveState::DriveState_OFF); + + ::MessageMonitorData messageMonitorData{ + true, strjointValues, + true, strjointValues, + true, strjointValues, + true, strjointValues, + true, strjointValues, + true, ::TimeStamp{1,2} + }; + ::MessageIpoData ipoData{ + true, strjointValues, + true, clientCommandMode, + true, overlayType, + true, 4.0 + }; + ::Transformation transformation{ "Transformation", 12, {1,2,3,4,5,6,7,8,9,10,11,12}, true, ::TimeStamp{3,4}}; + /// Transformation requestedTransformations[5] = { transformation, transformation, transformation, transformation, transformation }; + ::MessageEndOf endOfMessageData{true, 32, true, ::Checksum{true, 32}}; + ::FRIMonitoringMessage friMonitoringMessage { + messageHeader, + true, robotInfo, + true, messageMonitorData, + true, connectionInfo, + true, ipoData, + 5, + { transformation, transformation, transformation, transformation, transformation }, + true, endOfMessageData}; + grl::TimeEvent time_event_stamp; + std::vector> kukaiiwaStateVec; + std::size_t builder_size_bytes = 0; + std::size_t previous_size = 0; + + + while(!signalStatusG && OK && builder_size_bytes < single_buffer_limit_bytes ) + { + // grl::robot::arm::copy(friMonitoringMessage, armState); + flatbuffers::Offset controlState = grl::toFlatBuffer(*fbbP, basename, sequenceNumber++, duration, armState, armControlMode); + flatbuffers::Offset setCartesianImpedance = grl::toFlatBuffer(*fbbP, cart_stiffness_, cart_damping_, + nullspace_stiffness_, nullspace_damping_, cart_max_path_deviation_, cart_max_ctrl_vel_, cart_max_ctrl_force_, max_control_force_stop_); + flatbuffers::Offset setJointImpedance = grl::toFlatBuffer(*fbbP, joint_stiffness_, joint_damping_); + flatbuffers::Offset setSmartServo = grl::toFlatBuffer(*fbbP, joint_AccelerationRel_, joint_VelocityRel_, updateMinimumTrajectoryExecutionTime, minimumTrajectoryExecutionTime); + flatbuffers::Offset FRIConfig = grl::toFlatBuffer(*fbbP, overlayType, connectionInfo, false, 3501, false, 3502); + + std::vector> tools; + std::vector> processData; + + for(int i=0; i<7; i++) { + std::string linkname = "Link" + std::to_string(i); + std::string parent = i==0?"Base":("Link" + std::to_string(i-1)); + grl::flatbuffer::Pose pose = grl::flatbuffer::Pose(grl::flatbuffer::Vector3d(i,i,i), grl::flatbuffer::Quaternion(i,i,i,i)); + grl::flatbuffer::Inertia inertia = grl::flatbuffer::Inertia(1, pose, 1, 2, 3, 4, 5, 6); + flatbuffers::Offset linkObject = grl::toFlatBuffer(*fbbP, linkname, parent, pose, inertia); + tools.push_back(linkObject); + processData.push_back( + grl::toFlatBuffer(*fbbP, + "dataType"+ std::to_string(i), + "defaultValue"+ std::to_string(i), + "displayName"+ std::to_string(i), + "id"+ std::to_string(i), + "min"+ std::to_string(i), + "max"+ std::to_string(i), + "unit"+ std::to_string(i), + "value"+ std::to_string(i))); + } + + + // copy the state data into a more accessible object + /// TODO(ahundt) switch from this copy to a non-deprecated call + + flatbuffers::Offset kukaiiwaArmConfiguration = grl::toFlatBuffer( + *fbbP, + RobotName, + commandInterface, + monitorInterface, + clientCommandMode, + overlayType, + controlMode, + setCartesianImpedance, + setJointImpedance, + setSmartServo, + FRIConfig, + tools, + processData, + "currentMotionCenter", + true); + + flatbuffers::Offset friMessageLog = grl::toFlatBuffer( + *fbbP, + ::FRISessionState::FRISessionState_IDLE, + ::FRIConnectionQuality::FRIConnectionQuality_POOR, + controlMode, + friMonitoringMessage, + time_event_stamp); + grl::flatbuffer::Wrench cartesianWrench{grl::flatbuffer::Vector3d(1,1,1),grl::flatbuffer::Vector3d(1,1,1),grl::flatbuffer::Vector3d(1,1,1)}; + + + flatbuffers::Offset jointStatetab = grl::toFlatBuffer(*fbbP, jointValues, jointValues, jointValues, jointValues); + grl::flatbuffer::Pose cartesianFlangePose = grl::flatbuffer::Pose(grl::flatbuffer::Vector3d(1,1,1), grl::flatbuffer::Quaternion(2,3,4,5)); + flatbuffers::Offset kukaiiwaMonitorState = grl::toFlatBuffer( + *fbbP, + jointStatetab, // flatbuffers::Offset &measuredState, + cartesianFlangePose, + jointStatetab, // flatbuffers::Offset &jointStateReal, + jointStatetab, // flatbuffers::Offset &jointStateInterpolated, + jointStatetab, // flatbuffers::Offset &externalState, + ::FRISessionState::FRISessionState_IDLE, + ::OperationMode::OperationMode_TEST_MODE_1, + cartesianWrench); + std::vector torqueSensorLimits(7,0.5); + flatbuffers::Offset monitorConfig = grl::toFlatBuffer( + *fbbP, + "hardvareVersion", + torqueSensorLimits, + true, + false, + processData); + flatbuffers::Offset kukaiiwaState = grl::toFlatBuffer( + *fbbP, + RobotName, + destination, + source, + time_event_stamp, + true, controlState, + true, kukaiiwaArmConfiguration, + true, kukaiiwaMonitorState, + false, monitorConfig, + friMessageLog); + + kukaiiwaStateVec.push_back(kukaiiwaState); + + builder_size_bytes = fbbP->GetSize(); + std::size_t newData = builder_size_bytes - previous_size; + previous_size = builder_size_bytes; + // std::cout<< "single message data size (bytes): " << newData << " Buffer size: " << builder_size_bytes/MegaByte <<" MB" << std::endl; + } + + //std::cout<< "kukaiiwaStateVec:" << kukaiiwaStateVec.size() << std::endl; + + flatbuffers::Offset kukaStates = grl::toFlatBuffer(*fbbP, kukaiiwaStateVec); + + grl::flatbuffer::FinishKUKAiiwaStatesBuffer(*fbbP, kukaStates); + + uint8_t *buf = fbbP->GetBufferPointer(); + std::size_t bufsize = fbbP->GetSize(); + + /// To expand the capacity of a single buffer, _max_tables is set to 10000000 + flatbuffers::uoffset_t _max_depth = 64; + flatbuffers::uoffset_t _max_tables = 10000000; + flatbuffers::Verifier verifier(buf, bufsize, _max_depth, _max_tables); + OK = OK && grl::flatbuffer::VerifyKUKAiiwaStatesBuffer(verifier); + assert(OK && "VerifyKUKAiiwaStatesBuffer"); + + //std::cout << "Buffer size: " << bufsize << std::endl; + + std::string binary_file_path = "Kuka_test_binary.iiwa"; + std::string json_file_path = "kuka_test_text.json"; + + // Get the current working directory + std::string fbs_filename("KUKAiiwa.fbs"); + + OK = OK && grl::SaveFlatBufferFile( + buf, + fbbP->GetSize(), + binary_file_path, + fbs_filename, + json_file_path); + assert(OK && "SaveFlatBufferFile"); + } + std::cout << "End of the program" << std::endl; + +} + +BOOST_AUTO_TEST_SUITE_END() diff --git a/test/HandEyeCalibration_test.cpp b/test/HandEyeCalibration_test.cpp index 3d005d3..51c7fc6 100644 --- a/test/HandEyeCalibration_test.cpp +++ b/test/HandEyeCalibration_test.cpp @@ -8,12 +8,21 @@ #include "camodocal/calib/HandEyeCalibration.h" #include "camodocal/EigenUtils.h" +// boost::filesystem +#include +#include +#include + + BOOST_AUTO_TEST_SUITE(HandEyeCalibration_test) BOOST_AUTO_TEST_CASE(FullMotion) { - camodocal::HandEyeCalibration::setVerbose(false); + std::string currentPath = boost::filesystem::current_path().string(); + std::cout << "${workspaceFolder}: " << currentPath << std::endl; + camodocal::HandEyeCalibration::setVerbose(false); + Eigen::Matrix4d H_12_expected = Eigen::Matrix4d::Identity(); H_12_expected.block<3,3>(0,0) = Eigen::AngleAxisd(0.4, Eigen::Vector3d(0.1, 0.2, 0.3).normalized()).toRotationMatrix(); H_12_expected.block<3,1>(0,3) << 0.5, 0.6, 0.7; diff --git a/test/KukaFRITest.cpp b/test/KukaFRITest.cpp index 55732c8..5b91ab5 100644 --- a/test/KukaFRITest.cpp +++ b/test/KukaFRITest.cpp @@ -19,6 +19,13 @@ #include +volatile std::sig_atomic_t signalStatusG = 0; + +// called when the user presses ctrl+c +void signal_handler(int signal) +{ + signalStatusG = signal; +} using boost::asio::ip::udp; @@ -42,131 +49,162 @@ enum class DriverToUse int main(int argc, char* argv[]) { - bool debug = true; - int print_every_n = 100; - std::size_t q_size = 4096; //queue size must be power of 2 - spdlog::set_async_mode(q_size); - std::shared_ptr loggerPG; - try { loggerPG = spdlog::stdout_logger_mt("console"); } catch (spdlog::spdlog_ex ex) { loggerPG = spdlog::get("console"); } - - grl::periodic<> callIfMinPeriodPassed; - HowToMove howToMove = HowToMove::absolute_position_with_relative_rotation;//HowToMove::absolute_position; HowToMove::relative_position; - DriverToUse driverToUse = DriverToUse::kuka_driver_high_level_class; - - try - { - std::string localhost("192.170.10.100"); - std::string localport("30200"); - std::string remotehost("192.170.10.2"); - std::string remoteport("30200"); - - std::cout << "argc: " << argc << "\n"; - if (argc !=5 && argc !=1) - { - loggerPG->error("Usage: ", argv[0], " \n"); - return 1; + bool debug = true; + int print_every_n = 100; + std::size_t q_size = 4096; //queue size must be power of 2 + spdlog::set_async_mode(q_size); + std::shared_ptr loggerPG; + try { + loggerPG = spdlog::stdout_logger_mt("console"); } - - if(argc ==5){ - localhost = std::string(argv[1]); - localport = std::string(argv[2]); - remotehost = std::string(argv[3]); - remoteport = std::string(argv[4]); + catch (spdlog::spdlog_ex ex) { + loggerPG = spdlog::get("console"); } - std::cout << "using: " << argv[0] << " " << localhost << " " << localport << " " << remotehost << " " << remoteport << "\n"; - - boost::asio::io_service io_service; - - std::shared_ptr friData(std::make_shared(7)); - std::chrono::time_point startTime; - - BOOST_VERIFY(friData); - - double delta = -0.0005; - double delta_sum = 0; - /// consider moving joint angles based on time - int joint_to_move = 6; - loggerPG->warn("WARNING: YOU COULD DAMAGE OR DESTROY YOUR KUKA ROBOT {}{}{}{}{}{}", - "if joint angle delta variable is too large with respect to ", - "the time it takes to go around the loop and change it. ", - "Current delta (radians/update): ", delta, " Joint to move: ", joint_to_move); - - std::vector ipoJointPos(7,0); - std::vector jointOffset(7,0); // length 7, value 0 - boost::container::static_vector jointStateToCommand(7,0); - - // Absolute goal position to travel to in some modes of HowToMove - // Set all 7 joints to go to a position 1 radian from the center - std::vector absoluteGoalPos(7,0.2); - - /// TODO(ahundt) remove deprecated arm state from here and implementation - grl::robot::arm::KukaState armState; - std::unique_ptr lowLevelStepAlgorithmP; - - // Need to tell the system how long in milliseconds it has to reach the goal - // or it will never move! - std::size_t goal_position_command_time_duration = 4; - lowLevelStepAlgorithmP.reset(new grl::robot::arm::LinearInterpolation()); - // RobotModel (options are KUKA_LBR_IIWA_14_R820, KUKA_LBR_IIWA_7_R800) see grl::robot::arm::KukaState::KUKA_LBR_IIWA_14_R820 - - std::shared_ptr> highLevelDriverClassP; - - if(driverToUse == DriverToUse::low_level_fri_class) - { - /// @todo TODO(ahundt) BUG: Need way to supply time to reach specified goal for position control and eliminate this allocation internally in the kuka driver. See similar comment in KukaFRIDriver.hpp - /// IDEA: PASS A LOW LEVEL STEP ALGORITHM PARAMS OBJECT ON EACH UPDATE AND ONLY ONE INSTANCE OF THE ALGORITHM OBJECT ITSELF - highLevelDriverClassP = std::make_shared>(io_service, - std::make_tuple("KUKA_LBR_IIWA_14_R820",localhost,localport,remotehost,remoteport/*,4 ms per tick*/,grl::robot::arm::KukaFRIClientDataDriver::run_automatically)); - - } - - std::shared_ptr socketP; - - if(driverToUse == DriverToUse::low_level_fri_function) - { - - socketP = std::make_shared(io_service, boost::asio::ip::udp::endpoint(boost::asio::ip::address::from_string(localhost), boost::lexical_cast(localport))); + grl::periodic<> callIfMinPeriodPassed; + // defines how the robot will move when this test is executed + HowToMove howToMove = HowToMove::absolute_position_with_relative_rotation;//HowToMove::absolute_position; HowToMove::relative_position; - boost::asio::ip::udp::resolver resolver(io_service); - boost::asio::ip::udp::endpoint endpoint = *resolver.resolve({boost::asio::ip::udp::v4(), remotehost, remoteport}); - socketP->connect(endpoint); + // change which part of the driver is being tested. + // It is divided into several levels of functionality + // and the java program you need to execute varies based on + // which selection you make. + // + // Run GRL_Driver on the Kuka teach pendant with: + // + // DriverToUse::kuka_driver_high_level_class + // + // Run FRI_HoldsPosition_command on the KUKA teach pendant with: + // + // DriverToUse::low_level_fri_function, + // DriverToUse::low_level_fri_class, + // + DriverToUse driverToUse = DriverToUse::kuka_driver_high_level_class; - /// @todo maybe there is a more convienient way to set this that is easier for users? perhaps initializeClientDataForiiwa()? - friData->expectedMonitorMsgID = KUKA::LBRState::LBRMONITORMESSAGEID; - } - - std::shared_ptr kukaDriverP; - - if(driverToUse == DriverToUse::kuka_driver_high_level_class) + try { - grl::robot::arm::KukaDriver::Params params = std::make_tuple( - "Robotiiwa" , // RobotName, - "KUKA_LBR_IIWA_14_R820" , // RobotModel (options are KUKA_LBR_IIWA_14_R820, KUKA_LBR_IIWA_7_R800) - "0.0.0.0" , // LocalUDPAddress - "30010" , // LocalUDPPort - "172.31.1.147" , // RemoteUDPAddress - "192.170.10.100" , // LocalHostKukaKoniUDPAddress, - "30200" , // LocalHostKukaKoniUDPPort, - remotehost , // RemoteHostKukaKoniUDPAddress, - remoteport , // RemoteHostKukaKoniUDPPort - "FRI" , // KukaCommandMode (options are FRI, JAVA) - "FRI" // KukaMonitorMode (options are FRI, JAVA) - ); - /// @todo TODO(ahundt) Currently assumes ip address - kukaDriverP=std::make_shared(params); - kukaDriverP->construct(); - // Default to joint servo mode for commanding motion - kukaDriverP->set(grl::flatbuffer::ArmState::MoveArmJointServo); - kukaDriverP->set(goal_position_command_time_duration,grl::time_duration_command_tag()); - std::cout << "KUKA COMMAND MODE: " << std::get(params) << "\n"; - - } - + std::string localhost("192.170.10.100"); + std::string localport("30200"); + std::string remotehost("192.170.10.2"); + std::string remoteport("30200"); + int single_buffer_limit = 2; + + std::cout << "argc: " << argc << "\n"; + if (argc !=5 && argc !=1) + { + loggerPG->error("Usage: ", argv[0], " \n"); + return 1; + } + + if(argc ==5){ + localhost = std::string(argv[1]); + localport = std::string(argv[2]); + remotehost = std::string(argv[3]); + remoteport = std::string(argv[4]); + } + + std::cout << "using: " << argv[0] << " " << localhost << " " << localport << " " << remotehost << " " << remoteport << "\n"; + // A single class for an I/O service object. + boost::asio::io_service io_service; + + std::shared_ptr friData(std::make_shared(7)); + /// std::chrono::time_point startTime; + cartographer::common::Time startTime; + + BOOST_VERIFY(friData); + + double delta = -0.0005; + double delta_sum = 0; + /// consider moving joint angles based on time + int joint_to_move = 6; + loggerPG->warn("WARNING: THIS PROGRAM WILL ACTUALLY MOVE YOUR KUKA ROBOT!!!", + "YOU COULD INJURE YOURSELF, THE OTHERS AROUND YOU, ", + "AND DAMAGE OR DESTROY YOUR KUKA ROBOT. TAKE APPROPRIATE PRECAUTIONS. YOU ARE RESPONSIBLE. {}{}{}{}{}{}", + "if joint angle delta variable is too large with respect to ", + "the time it takes to go around the loop and change it. ", + "Current delta (radians/update): ", delta, " Joint to move: ", joint_to_move); + + std::vector ipoJointPos(7,0); + std::vector jointOffset(7,0); // length 7, value 0 + boost::container::static_vector jointStateToCommand(7,0); + + // Absolute goal position to travel to in some modes of HowToMove + // Set all 7 joints to go to a position 1 radian from the center + std::vector absoluteGoalPos(7,0.2); + + /// TODO(ahundt) remove deprecated arm state from here and implementation + grl::robot::arm::KukaState armState; + std::unique_ptr lowLevelStepAlgorithmP; - unsigned int num_missed = 0; + // Need to tell the system how long in milliseconds it has to reach the goal + // or it will never move! + std::size_t goal_position_command_time_duration = 4; + lowLevelStepAlgorithmP.reset(new grl::robot::arm::LinearInterpolation()); + // RobotModel (options are KUKA_LBR_IIWA_14_R820, KUKA_LBR_IIWA_7_R800) see grl::robot::arm::KukaState::KUKA_LBR_IIWA_14_R820 - for (std::size_t i = 0;;++i) { + std::shared_ptr> highLevelDriverClassP; + + if(driverToUse == DriverToUse::low_level_fri_class) + { + /// @todo TODO(ahundt) BUG: Need way to supply time to reach specified goal for position control and eliminate this allocation internally in the kuka driver. See similar comment in KukaFRIDriver.hpp + /// IDEA: PASS A LOW LEVEL STEP ALGORITHM PARAMS OBJECT ON EACH UPDATE AND ONLY ONE INSTANCE OF THE ALGORITHM OBJECT ITSELF + highLevelDriverClassP = std::make_shared>( + io_service, + std::make_tuple("KUKA_LBR_IIWA_14_R820", + localhost, + localport, + remotehost, + remoteport/*,4 ms per tick*/, + grl::robot::arm::KukaFRIClientDataDriver::run_automatically)); + } + + std::shared_ptr socketP; + + if(driverToUse == DriverToUse::low_level_fri_function) + { + + socketP = std::make_shared(io_service, boost::asio::ip::udp::endpoint(boost::asio::ip::address::from_string(localhost), boost::lexical_cast(localport))); + + boost::asio::ip::udp::resolver resolver(io_service); + boost::asio::ip::udp::endpoint endpoint = *resolver.resolve({boost::asio::ip::udp::v4(), remotehost, remoteport}); + socketP->connect(endpoint); + + /// @todo maybe there is a more convienient way to set this that is easier for users? perhaps initializeClientDataForiiwa()? + friData->expectedMonitorMsgID = KUKA::LBRState::LBRMONITORMESSAGEID; + } + + std::shared_ptr kukaDriverP; + + if(driverToUse == DriverToUse::kuka_driver_high_level_class) + { + grl::robot::arm::KukaDriver::Params params = std::make_tuple( + "Robotiiwa" , // RobotName, + "KUKA_LBR_IIWA_14_R820" , // RobotModel (options are KUKA_LBR_IIWA_14_R820, KUKA_LBR_IIWA_7_R800) + "0.0.0.0" , // LocalUDPAddress + "30010" , // LocalUDPPort + "172.31.1.147" , // RemoteUDPAddress + "192.170.10.100" , // LocalHostKukaKoniUDPAddress, + "30200" , // LocalHostKukaKoniUDPPort, + remotehost , // RemoteHostKukaKoniUDPAddress, + remoteport , // RemoteHostKukaKoniUDPPort + "FRI" , // KukaCommandMode (options are FRI, JAVA) + "FRI" // KukaMonitorMode (options are FRI, JAVA) + ); + /// @todo TODO(ahundt) Currently assumes ip address + kukaDriverP = std::make_shared(params); + kukaDriverP->construct(); + // Default to joint servo mode for commanding motion + kukaDriverP->set(grl::flatbuffer::ArmState::MoveArmJointServo); + kukaDriverP->set(goal_position_command_time_duration,grl::time_duration_command_tag()); + std::cout << "KUKA COMMAND MODE: " << std::get(params) << "\n"; + /// kukaDriverP->start_recording(); + + } + std::size_t num_missed = 0; + std::size_t maximum_consecutive_missed_updates_before_warning = 100000; + std::size_t maximum_consecutive_missed_updates_before_exit = 100000000; + // run until the user kills the program with ctrl+c + for (std::size_t i = 0; !signalStatusG; ++i) { /// Save the interpolated joint position from the previous update as the base for some motions /// The interpolated position is where the JAVA side is commanding, @@ -178,29 +216,47 @@ int main(int argc, char* argv[]) boost::system::error_code send_ec, recv_ec; std::size_t send_bytes_transferred = 0, recv_bytes_transferred = 0; bool haveNewData = false; - + grl::TimeEvent time_event_stamp; if(driverToUse == DriverToUse::low_level_fri_class) { - auto step_commandP = std::make_shared(std::make_tuple(jointStateToCommand,goal_position_command_time_duration)); - haveNewData = !highLevelDriverClassP->update_state(step_commandP, friData, recv_ec, recv_bytes_transferred, send_ec, send_bytes_transferred); + auto step_commandP = std::make_shared(std::make_tuple(jointStateToCommand, goal_position_command_time_duration)); + + haveNewData = !highLevelDriverClassP->update_state(step_commandP, + friData, + recv_ec, + recv_bytes_transferred, + send_ec, + send_bytes_transferred, + time_event_stamp); } - + if(driverToUse == DriverToUse::low_level_fri_function) { - grl::robot::arm::update_state(*socketP,*lowLevelStepAlgorithmP,*friData,send_ec,send_bytes_transferred, recv_ec, recv_bytes_transferred); + /// This update_state function is different from the above one. + /// They both are defined in KukaFRIdriver.hpp. + /// Also should the argument time_event_stam be the same with above one? + grl::robot::arm::update_state(*socketP, + *lowLevelStepAlgorithmP, + *friData, + send_ec, + send_bytes_transferred, + recv_ec, + recv_bytes_transferred, + time_event_stamp); } - + if(driverToUse == DriverToUse::kuka_driver_high_level_class) { - + kukaDriverP->set( jointStateToCommand, grl::revolute_joint_angle_open_chain_command_tag()); + kukaDriverP->start_recording(single_buffer_limit); kukaDriverP->run_one(); } // if data didn't arrive correctly, skip and try again if(send_ec || recv_ec ) { - + loggerPG->error("receive error: ", recv_ec, "receive bytes: ", recv_bytes_transferred, " send error: ", send_ec, " send bytes: ", send_bytes_transferred, " iteration: ", i); if(driverToUse == DriverToUse::low_level_fri_class) std::this_thread::sleep_for(std::chrono::milliseconds(1)); continue; @@ -210,14 +266,20 @@ int main(int argc, char* argv[]) // but we can't process the new data so try updating again immediately. if(!haveNewData && !recv_bytes_transferred) { - if(driverToUse == DriverToUse::low_level_fri_class) std::this_thread::sleep_for(std::chrono::milliseconds(1)); - ++num_missed; - if(num_missed>10000) { - loggerPG->warn("No new data for ", num_missed, " milliseconds."); - break; - } else { - continue; - } + if(driverToUse == DriverToUse::low_level_fri_class || driverToUse == DriverToUse::kuka_driver_high_level_class) + { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + ++num_missed; + if(num_missed > maximum_consecutive_missed_updates_before_warning) { + loggerPG->warn("No new data for ", num_missed, " milliseconds."); + continue; + } else if(num_missed > maximum_consecutive_missed_updates_before_exit) { + loggerPG->warn("No new data for ", num_missed, " milliseconds. Max time limit hit, Exiting..."); + break; + } else { + continue; + } } else { num_missed = 0; } @@ -230,7 +292,7 @@ int main(int argc, char* argv[]) // setting howToMove to HowToMove::remain_stationary block causes the robot to simply sit in place, which seems to work correctly. Enabling it causes the joint to rotate. if (howToMove != HowToMove::remain_stationary && - grl::robot::arm::get(friData->monitoringMsg,KUKA::FRI::ESessionState()) == KUKA::FRI::COMMANDING_ACTIVE) + grl::robot::arm::get(friData->monitoringMsg,::FRISessionState()) == ::FRISessionState::FRISessionState_COMMANDING_ACTIVE) { callIfMinPeriodPassed.execution( [&armState,&jointOffset,&delta,&delta_sum,joint_to_move]() { @@ -248,9 +310,9 @@ int main(int argc, char* argv[]) }); } - KUKA::FRI::ESessionState sessionState = grl::robot::arm::get(friData->monitoringMsg,KUKA::FRI::ESessionState()); + ::FRISessionState sessionState = grl::robot::arm::get(friData->monitoringMsg,::FRISessionState()); // copy current joint position to commanded position - if (sessionState == KUKA::FRI::COMMANDING_WAIT || sessionState == KUKA::FRI::COMMANDING_ACTIVE) + if (sessionState == ::FRISessionState::FRISessionState_COMMANDING_WAIT || sessionState == ::FRISessionState::FRISessionState_COMMANDING_ACTIVE) { if (howToMove == HowToMove::relative_position) { // go to a position relative to the current position @@ -260,7 +322,7 @@ int main(int argc, char* argv[]) boost::copy ( absoluteGoalPos, jointStateToCommand.begin()); } else if (howToMove == HowToMove::absolute_position_with_relative_rotation) { // go to a position relative to the current position - + boost::transform ( absoluteGoalPos, jointOffset, jointStateToCommand.begin(), std::plus()); } } @@ -272,7 +334,8 @@ int main(int argc, char* argv[]) loggerPG->info( "position: {}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}", armState.position, " commanded Position: ", jointStateToCommand, - " us: ", std::chrono::duration_cast(armState.timestamp - startTime).count(), + /// " us: ", std::chrono::duration_cast(armState.timestamp - startTime).count(), + " us: ", std::chrono::duration_cast(armState.time_event_stamp.device_time - startTime).count(), " connectionQuality: ", EnumNameEConnectionQuality(armState.connectionQuality), " operationMode: ", EnumNameEOperationMode(armState.operationMode), " sessionState: ", EnumNameESessionState(armState.sessionState), @@ -281,8 +344,8 @@ int main(int argc, char* argv[]) " jointOffset: ", jointOffset); } - } - } + } + } catch (boost::exception &e) { std::string errmsg("If you get an error 'std::exception::what: bind: Can't assign requested address', check your network connection.\n\nKukaFRITest Main Test Loop Stopped:\n" + boost::diagnostic_information(e)); @@ -294,5 +357,6 @@ int main(int argc, char* argv[]) // Release and close all loggers spdlog::drop_all(); + std::cout << "End of the program" << std::endl; return 0; } diff --git a/test/KukaLBRiiwaVrepPluginTest.cpp b/test/KukaLBRiiwaVrepPluginTest.cpp index 8f2fa57..4ee914e 100644 --- a/test/KukaLBRiiwaVrepPluginTest.cpp +++ b/test/KukaLBRiiwaVrepPluginTest.cpp @@ -3,45 +3,46 @@ // system includes #include + #include #include #include #include - +#include //// local includes #include "grl/vrep/KukaLBRiiwaVrepPlugin.hpp" BOOST_AUTO_TEST_SUITE(KukaLBRiiwaVrepPluginTest) BOOST_AUTO_TEST_CASE(initialization){ -auto plugin = std::make_shared(); -//BOOST_CHECK_THROW (std::make_shared(),boost::exception); -// plugin->construct() // shouldn't work, because vrep isn't around to get handles -//// uncomment to test error output -// try { -// BOOST_LOG_TRIVIAL(info) << "Starting KUKA LBR iiwa plugin connection to Kuka iiwa\n"; -// auto kukaPluginPG = std::make_shared(); -// kukaPluginPG->construct(); -// } catch (boost::exception& e){ -// // log the error and print it to the screen, don't release the exception -// BOOST_LOG_TRIVIAL(error) << boost::diagnostic_information(e); -// } + auto plugin = std::make_shared(); + BOOST_CHECK_THROW (std::make_shared(),boost::exception); + plugin->construct(); // shouldn't work, because vrep isn't around to get handles + // uncomment to test error output + try { + // BOOST_LOG_TRIVIAL(info) << "Starting KUKA LBR iiwa plugin connection to Kuka iiwa\n"; + auto kukaVrepPluginPG = std::make_shared(); + kukaVrepPluginPG->construct(); + } catch (boost::exception& e){ + // log the error and print it to the screen, don't release the exception + // BOOST_LOG_TRIVIAL(error) << boost::diagnostic_information(e); + } } BOOST_AUTO_TEST_CASE(connectToFake){ - std::vector jointHandles; + std::vector jointNames; - jointHandles.push_back("LBR_iiwa_14_R820_joint1"); // Joint1Handle, - jointHandles.push_back("LBR_iiwa_14_R820_joint2"); // Joint2Handle, - jointHandles.push_back("LBR_iiwa_14_R820_joint3"); // Joint3Handle, - jointHandles.push_back("LBR_iiwa_14_R820_joint4"); // Joint4Handle, - jointHandles.push_back("LBR_iiwa_14_R820_joint5"); // Joint5Handle, - jointHandles.push_back("LBR_iiwa_14_R820_joint6"); // Joint6Handle, - jointHandles.push_back("LBR_iiwa_14_R820_joint7"); // Joint7Handle, + jointNames.push_back("LBR_iiwa_14_R820_joint1"); // Joint1Handle, + jointNames.push_back("LBR_iiwa_14_R820_joint2"); // Joint2Handle, + jointNames.push_back("LBR_iiwa_14_R820_joint3"); // Joint3Handle, + jointNames.push_back("LBR_iiwa_14_R820_joint4"); // Joint4Handle, + jointNames.push_back("LBR_iiwa_14_R820_joint5"); // Joint5Handle, + jointNames.push_back("LBR_iiwa_14_R820_joint6"); // Joint6Handle, + jointNames.push_back("LBR_iiwa_14_R820_joint7"); // Joint7Handle, auto config = std::make_tuple( - jointHandles , // JointHandles, + jointNames , // JointHandles, "RobotFlangeTip" , // RobotFlangeTipHandle, "RobotMillTip" , // RobotTipHandle, "RobotMillTipTarget" , // RobotTargetHandle, @@ -58,7 +59,12 @@ BOOST_AUTO_TEST_CASE(connectToFake){ "FRI" , // KukaMonitorMode (options are FRI, JAVA) "IK_Group1_iiwa" // IKGroupName ); -auto plugin = std::make_shared(config); + auto plugin = std::make_shared(config); + std::shared_ptr VrepRobotArmDriverSimulatedP_; + // Get the arm that will be used to generate simulated results to command robot + // the "base" of this ik is Robotiiwa + VrepRobotArmDriverSimulatedP_ = std::make_shared(); + VrepRobotArmDriverSimulatedP_->construct(); } BOOST_AUTO_TEST_SUITE_END() diff --git a/test/fusionTrackTest.cpp b/test/fusionTrackTest.cpp index 46b6a01..0b9fd22 100644 --- a/test/fusionTrackTest.cpp +++ b/test/fusionTrackTest.cpp @@ -35,8 +35,8 @@ BOOST_AUTO_TEST_CASE(initialization) //// uncomment to test error output // try { // BOOST_LOG_TRIVIAL(info) << "Starting KUKA LBR iiwa plugin connection to Kuka iiwa\n"; - // auto kukaPluginPG = std::make_shared(); - // kukaPluginPG->construct(); + // auto kukaVrepPluginPG = std::make_shared(); + // kukaVrepPluginPG->construct(); // } catch (boost::exception& e){ // // log the error and print it to the screen, don't release the exception // BOOST_LOG_TRIVIAL(error) << boost::diagnostic_information(e); @@ -93,7 +93,7 @@ BOOST_AUTO_TEST_CASE(runRepeatedly) flatbuffers::Offset LogKUKAiiwaFusionTrack = grl::toFlatBuffer(fbb, states); // Finish a buffer with a given root object fbb.Finish(LogKUKAiiwaFusionTrack); - + // print byte data for debugging: //flatbuffers::SaveFile("test.flik", fbb.GetBufferPointer(), fbb.GetSize()); std::string filename = "test.flik"; diff --git a/test/kukaiiwaURDF.h b/test/kukaiiwaURDF.h new file mode 100644 index 0000000..f358cf2 --- /dev/null +++ b/test/kukaiiwaURDF.h @@ -0,0 +1,492 @@ +/* Copyright 2015-2017 CNRS-UM LIRMM, CNRS-AIST JRL + * + * This file is part of mc_rbdyn_urdf. + * + * mc_rbdyn_urdf is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * mc_rbdyn_urdf is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with mc_rbdyn_urdf. If not, see . + */ +/** + * THIS IS A TEMPORAL FILE, AFTER FINISHING THE TEST, REMOVE IT. + * @CHUNTING + */ +#pragma once + +#include +#include + +#include "RBDyn/FK.h" + + +std::string XYZSarmUrdf( +R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + /iiwa + + + + + Gazebo/Grey + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Grey + 0.2 + 0.2 + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + +)"); + +namespace mc_rbdyn_urdf +{ +bool operator==( const Geometry::Mesh& m1, const Geometry::Mesh& m2) +{ + return m1.scale == m2.scale && m1.filename == m2.filename; +} + +bool operator==( const Geometry::Box& b1, const Geometry::Box& b2) +{ + return b1.size == b2.size; +} + +bool operator==( const Geometry::Sphere& b1, const Geometry::Sphere& b2) +{ + return b1.radius == b2.radius; +} + +bool operator==( const Geometry::Cylinder& b1, const Geometry::Cylinder& b2) +{ + return b1.radius == b2.radius && b1.length == b2.length; +} + +bool operator==(const Geometry& g1, const Geometry& g2) +{ + return g1.type == g2.type && g1.data == g2.data; +} +bool operator==(const Visual& v1, const Visual& v2) { + return v1.name == v2.name && v1.origin == v2.origin && v1.geometry == v2.geometry; +} +} /* mc_rbdyn_urdf */ + + +const double TOL = 1e-6; \ No newline at end of file diff --git a/test/readFlatbufferTest.cpp b/test/readFlatbufferTest.cpp new file mode 100644 index 0000000..91098ea --- /dev/null +++ b/test/readFlatbufferTest.cpp @@ -0,0 +1,273 @@ + +// Library includes +#define FLATBUFFERS_DEBUG_VERIFICATION_FAILURE +#include "flatbuffers/util.h" +#include "grl/flatbuffer/ParseflatbuffertoCSV.hpp" +#include +#include "grl/flatbuffer/FusionTrack_generated.h" +#include "grl/flatbuffer/Time_generated.h" +#include "grl/flatbuffer/LogKUKAiiwaFusionTrack_generated.h" +#include "grl/time.hpp" + + +#include +#include +#include +#include + +#include +#include +/// Boost to create an empty folder +// boost::filesystem +#include +#include +#include + +#include +/// Command of spliting a large file into some smaller ones. +/// split -C 200m --numeric-suffixes 2018_02_28_16_39_13_FusionTrack.json 2018_02_28_16_39_13_FusionTrack +/// The command to get the json file from flatbuffer binary file, these two files should be located in the same folder. +/// flatc -I . --json LogKUKAiiwaFusionTrack.fbs -- 2018_04_13_16_20_28_FusionTrack.flik +/// flatc -I . --json KUKAiiwa.fbs -- 2018_04_13_16_20_28_Kukaiiwa.iiwa + +int main(int argc, char* argv[]) +{ + std::string kukaTimeStamp("2018_04_13_16_20_28_Kukaiiwa.iiwa"); + std::string FTTimeStamp("2018_04_13_16_20_28_FusionTrack.flik"); + std::string URDFModrl("Robone_KukaLBRiiwa.urdf"); + /// Define the csv file names + if(argc == 2){ + kukaTimeStamp = std::string(argv[0]); + FTTimeStamp = std::string(argv[1]); + std::cout << "Arguments: " << kukaTimeStamp << " " << FTTimeStamp << std::endl; + } + /// Create a new folder in the name of time stamp for the generated csv files. + std::string homePath = std::getenv("HOME"); + std::string vrepPath = homePath + "/src/V-REP_PRO_EDU_V3_4_0_Linux/data/"; + std::string kukaBinaryfile = vrepPath + kukaTimeStamp; + + std::string fusiontrackBinaryfile = vrepPath + FTTimeStamp; + std::string urdfFile = vrepPath + URDFModrl; + std::string foldtimestamp = current_date_and_time_string(); // Write the generated files into a new fold + boost::filesystem::path dir{vrepPath+foldtimestamp}; + boost::filesystem::create_directory(vrepPath+foldtimestamp); + /* + 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. + */ + std::string KUKA_FRI_CSVfilename = vrepPath + foldtimestamp + "/KUKA_FRIMessage.csv"; + std::string KUKA_TimeEvent_CSV = vrepPath + foldtimestamp + "/KUKA_TimeEvent.csv"; + std::string M_Joint_CSV = vrepPath + foldtimestamp + "/KUKA_Measured_Joint.csv"; + std::string C_Joint_CSV = vrepPath + foldtimestamp + "/KUKA_Command_Joint.csv"; + std::string KUKA_Pose_CSV = vrepPath + foldtimestamp + "/KUKA_Pose.csv"; + std::string KUKA_Inverse_Pose_CSV = vrepPath + foldtimestamp + "/Inverse_KUKA_Pose.csv"; + std::string FudicialToRobotPose_CSV = vrepPath + foldtimestamp + "/FudicialToRobot_Pose.csv"; + std::string FudicialToFTPose_CSV = vrepPath + foldtimestamp + "/FudicialToFT_Pose.csv"; + + std::string FT_TimeEvent_CSV = vrepPath + foldtimestamp + "/FT_TimeEvent.csv"; + std::string FT_Marker22_CSV = vrepPath + foldtimestamp + "/FT_Pose_Marker22.csv"; + std::string FT_Marker55_CSV = vrepPath + foldtimestamp + "/FT_Pose_Marker55.csv"; + std::string FT_Marker50000_CSV = vrepPath + foldtimestamp + "/FT_Pose_Marker50000.csv"; + + std::string FTKUKA_TimeEvent_CSV = vrepPath + foldtimestamp + "/FTKUKA_TimeEvent.csv"; + + std::string README_LOG = vrepPath + foldtimestamp + "/readme.txt"; + + auto readme = boost::filesystem::path(README_LOG); + if(boost::filesystem::exists(readme)){ + boost::filesystem::remove(readme); + std::cout << readme << " has been removed..." << std::endl; + } + boost::filesystem::ofstream ofs(readme, std::ios_base::app | std::ios_base::out); + ofs << "Read the binary files: \n"; + ofs << kukaBinaryfile << "\n" << fusiontrackBinaryfile<< "\n"; + + + uint32_t markerID_22 = 22; + uint32_t markerID_55 = 55; + uint32_t markerID_50000 = 50000; + /// Get the object pointer to read data from flatbuffer + fbs_tk::Root logKUKAiiwaFusionTrackP = fbs_tk::open_root(fusiontrackBinaryfile); + fbs_tk::Root kukaStatesP = fbs_tk::open_root(kukaBinaryfile); + std::size_t FT_size = grl::getStateSize(logKUKAiiwaFusionTrackP); + std::size_t kuka_size = grl::getStateSize(kukaStatesP); + + /// Create matrix to contain data + Eigen::MatrixXd markerPose_FT(FT_size, grl::col_Pose); + Eigen::MatrixXd markerTransform_FT = Eigen::MatrixXd::Zero(FT_size, grl::PK_Pose_Labels.size()); + grl::MatrixXd timeEventM_FT = grl::MatrixXd::Zero(FT_size, grl::Time_Labels.size()); + grl::MatrixXd timeEventM_Kuka = grl::MatrixXd::Zero(kuka_size, grl::Time_Labels.size()); + Eigen::VectorXd sequenceCounterVec = Eigen::VectorXd::Zero(kuka_size); + Eigen::VectorXd reflectedSequenceCounterVec = Eigen::VectorXd::Zero(kuka_size); + Eigen::MatrixXd measuredJointPosition = Eigen::MatrixXd::Zero(kuka_size, grl::jointNum); + Eigen::MatrixXd measuredTorque = Eigen::MatrixXd::Zero(kuka_size, grl::jointNum); + Eigen::MatrixXd commandedJointPosition = Eigen::MatrixXd::Zero(kuka_size, grl::jointNum); + Eigen::MatrixXd commandedTorque = Eigen::MatrixXd::Zero(kuka_size, grl::jointNum); + Eigen::MatrixXd externalTorque = Eigen::MatrixXd::Zero(kuka_size, grl::jointNum); + Eigen::MatrixXd jointStateInterpolated = Eigen::MatrixXd::Zero(kuka_size, grl::jointNum); + ///////////////////////////////////////////////////////////////////////////////////////////////////// + /// Get KUKA FRI message + //////////////////////////////////////////////////////////////////////////////////////////////////// + int ret = grl::getFRIMessage(kukaStatesP, + timeEventM_Kuka, + sequenceCounterVec, + reflectedSequenceCounterVec, + measuredJointPosition, + measuredTorque, + commandedJointPosition, + commandedTorque, + externalTorque, + jointStateInterpolated); + int validsize_22 = grl::getMarkerPose(logKUKAiiwaFusionTrackP, markerID_22, timeEventM_FT, markerPose_FT); + // int validsize_22 = grl::getMarkerMessage(logKUKAiiwaFusionTrackP, markerID_22, timeEventM_FT, markerPose_FT); + + ofs << "------Kuka_size: " << kuka_size << " FT_size: "<< FT_size << std::endl; + ofs <<"validsize_22: " << validsize_22 << std::endl; + std::size_t FT_time_size = timeEventM_FT.rows(); + std::size_t kuka_time_size = timeEventM_Kuka.rows(); + grl::MatrixXd timeEventM = grl::MatrixXd::Zero(FT_size, timeEventM_FT.cols()); + + int kuka_index = 0; + int FT_index = 0; + + // Skip the very beginning data,since the tracker starts to work once the scene is loaded in vrep. + // But kuka starts to work only after clicking on the start button. + // To combine the time from two devices, they should have the same starting time point. + // while(kuka_index timeEventM_FT(FT_index,grl::timeBaseline_index) ){ + // FT_index++; + // } + + auto initial_local_time = std::min(timeEventM_FT(FT_index,grl::timeBaseline_index), timeEventM_Kuka(kuka_index, grl::timeBaseline_index)); + auto initial_device_time_kuka = timeEventM_Kuka(kuka_index,grl::TimeType::device_time); + auto initial_device_time_FT = timeEventM_FT(FT_index,grl::TimeType::device_time); + ofs<< "Main initial_local_time: " << initial_local_time << std::endl; + timeEventM_FT.col(grl::TimeType::local_request_time) = timeEventM_FT.col(grl::TimeType::local_request_time) - initial_local_time * grl::VectorXd::Ones(FT_time_size); + timeEventM_FT.col(grl::TimeType::local_receive_time) = timeEventM_FT.col(grl::TimeType::local_receive_time) - initial_local_time * grl::VectorXd::Ones(FT_time_size); + timeEventM_FT.col(grl::TimeType::device_time) = timeEventM_FT.col(grl::TimeType::device_time) - initial_device_time_FT * grl::VectorXd::Ones(FT_time_size); + timeEventM_FT.col(grl::TimeType::time_Y) = timeEventM_FT.col(grl::TimeType::time_Y) - timeEventM_FT(FT_index, grl::TimeType::time_Y) * grl::VectorXd::Ones(FT_time_size); + + timeEventM_Kuka.col(grl::TimeType::local_request_time) = timeEventM_Kuka.col(grl::TimeType::local_request_time) - initial_local_time * grl::VectorXd::Ones(kuka_time_size); + timeEventM_Kuka.col(grl::TimeType::local_receive_time) = timeEventM_Kuka.col(grl::TimeType::local_receive_time) - initial_local_time * grl::VectorXd::Ones(kuka_time_size); + timeEventM_Kuka.col(grl::TimeType::device_time) = timeEventM_Kuka.col(grl::TimeType::device_time) - initial_device_time_kuka * grl::VectorXd::Ones(kuka_time_size); + timeEventM_Kuka.col(grl::TimeType::time_Y) = timeEventM_Kuka.col(grl::TimeType::time_Y) - timeEventM_Kuka(kuka_index, grl::TimeType::time_Y) * grl::VectorXd::Ones(kuka_time_size); + + ///////////////////////////////////////////////////////////////////////////////////////////////////// + /// Write KUKA FRI message to CSV + //////////////////////////////////////////////////////////////////////////////////////////////////// + + grl::writeFRIMessageToCSV(KUKA_FRI_CSVfilename, + timeEventM_Kuka, + sequenceCounterVec, + reflectedSequenceCounterVec, + measuredJointPosition, + measuredTorque, + commandedJointPosition, + commandedTorque, + externalTorque, + jointStateInterpolated, + kuka_index); + + timeEventM_Kuka.col(grl::TimeType::counterIdx) = std::move(sequenceCounterVec.cast()); + grl::writeTimeEventToCSV(KUKA_TimeEvent_CSV, timeEventM_Kuka, kuka_index); + std::vector kuka_labels = grl::Time_Labels; + kuka_labels.insert(std::end(kuka_labels), std::begin(grl::M_Pos_Joint_Labels), std::end(grl::M_Pos_Joint_Labels)); + // Write the joint angles into csv + grl::writeMatrixToCSV(M_Joint_CSV, kuka_labels, timeEventM_Kuka, measuredJointPosition); + std::copy(std::begin(grl::C_Pos_Joint_Labels), std::end(grl::C_Pos_Joint_Labels),std::begin(kuka_labels)+grl::Time_Labels.size()); + grl::writeMatrixToCSV(C_Joint_CSV, kuka_labels, timeEventM_Kuka, commandedJointPosition); + + auto strRobot = grl::getURDFModel(urdfFile); + rbd::MultiBody mb = strRobot.mb; + rbd::MultiBodyConfig mbc(mb); + rbd::MultiBodyGraph mbg(strRobot.mbg); + std::size_t nrJoints = mb.nrJoints(); + std::size_t nrBodies = strRobot.mb.nrBodies(); + std::vector jointNames; + std::cout <<"Joint Size: "<< nrJoints << std::endl; + for(int jointIdx=0; jointIdx0){ + mbc.q[jointIdx][0] = 0; + } + } + rbd::forwardKinematics(mb, mbc); + + + + std::vector PK_Pose_Labels = grl::getLabels(grl::LabelsType::Kuka_Pose); + // forward kinematic to get the of the end effector + std::vector PoseEE = grl::getPoseEE(measuredJointPosition); + // convert the sva::PTransformd to Plucker coordinate. + Eigen::MatrixXd PKPose = grl::getPluckerPose(PoseEE); + // Write the plucker coordinate into csv. + grl::writeMatrixToCSV(KUKA_Pose_CSV, PK_Pose_Labels, timeEventM_Kuka, PKPose); + // Invert the EE pose + std::vector inversePose = grl::invertPose(PoseEE); + // convert the sva::PTransformd to Plucker coordinate. + PKPose = Eigen::MatrixXd::Zero(PKPose.rows(), PKPose.cols()); + PKPose = grl::getPluckerPose(inversePose); + // Write the plucker coordinate into csv. + grl::writeMatrixToCSV(KUKA_Inverse_Pose_CSV, PK_Pose_Labels, timeEventM_Kuka, PKPose); + + ///////////////////////////////////////////////////////////////////////////////////////////////////// + /// Get and write Atracsys message to CSV + //////////////////////////////////////////////////////////////////////////////////////////////////// + // Get the pose of the marker 22. + // Since the bad data is skipped, the timeEventM_FT also needs to be filled out with the orinial time stamp. + std::vector FT_Labels_Pose = grl::getLabels(grl::LabelsType::FT_Pose); + grl::writeFTKUKAMessageToCSV(FTKUKA_TimeEvent_CSV, timeEventM_FT, timeEventM_Kuka, measuredJointPosition, markerPose_FT, FT_index, kuka_index); + grl::writeTimeEventToCSV(FT_TimeEvent_CSV, timeEventM_FT, FT_index); + + grl::writeMatrixToCSV(FT_Marker22_CSV, FT_Labels_Pose, timeEventM_FT, markerPose_FT, FT_index); + // Change it back to the original size + timeEventM_FT.resize(FT_size, grl::col_timeEvent); + markerPose_FT.resize(FT_size, grl::col_Pose); + timeEventM_FT = grl::MatrixXd::Zero(FT_size, grl::col_timeEvent); + markerPose_FT = Eigen::MatrixXd::Zero(FT_size, grl::col_Pose); + // Get the pose of the bone marker + int validsize_55 = grl::getMarkerPose(logKUKAiiwaFusionTrackP, markerID_55, timeEventM_FT, markerPose_FT); + // int validsize_55 = grl::getMarkerMessage(logKUKAiiwaFusionTrackP, markerID_55, timeEventM_FT, markerPose_FT); + ofs<<"validsize_55: " << validsize_55 << std::endl; + grl::regularizeTimeEvent(timeEventM_FT, initial_local_time, initial_device_time_FT, FT_index); + grl::writeMatrixToCSV(FT_Marker55_CSV, FT_Labels_Pose, timeEventM_FT, markerPose_FT, FT_index); + + // Change it back to the original size + timeEventM_FT.resize(FT_size, grl::col_timeEvent); + markerPose_FT.resize(FT_size, grl::col_Pose); + timeEventM_FT = grl::MatrixXd::Zero(FT_size, grl::col_timeEvent); + markerPose_FT = Eigen::MatrixXd::Zero(FT_size, grl::col_Pose); + // Get the pose of the bone marker + int validsize_50000 = grl::getMarkerPose(logKUKAiiwaFusionTrackP, markerID_50000, timeEventM_FT, markerPose_FT); + // int validsize_50000 = grl::getMarkerMessage(logKUKAiiwaFusionTrackP, markerID_50000, timeEventM_FT, markerPose_FT); + + ofs<<"validsize_50000: " << validsize_50000 << std::endl; + grl::regularizeTimeEvent(timeEventM_FT, initial_local_time, initial_device_time_FT, FT_index); + grl::writeMatrixToCSV(FT_Marker50000_CSV, FT_Labels_Pose, timeEventM_FT, markerPose_FT, FT_index); + + ofs.close(); + + + + + return 0; +} + + + + + + +