diff --git a/CMakeLists.txt b/CMakeLists.txt index b42fd83f..0df0ebee 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -72,6 +72,7 @@ set(TEST_LIST test_data_integrity test_endurance test_log + test_loop_latency ) foreach(test ${TEST_LIST}) diff --git a/example/tool/CMakeLists.txt b/example/tool/CMakeLists.txt deleted file mode 100644 index 0d89c335..00000000 --- a/example/tool/CMakeLists.txt +++ /dev/null @@ -1,49 +0,0 @@ -# use cmake 3 which supports targets -cmake_minimum_required(VERSION 3.1.3) - -# ===== CMAKE SETTINGS ===== -project(FlexivRdkTools) - -if( NOT CMAKE_BUILD_TYPE ) - set( CMAKE_BUILD_TYPE Release CACHE STRING - "Choose the type of build, options are: None Debug Release RelWithDebInfo MinSizeRel." - FORCE ) -endif() - -set(CMAKE_VERBOSE_MAKEFILE ON) -# set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin) - -set(THREADS_PREFER_PTHREAD_FLAG ON) -find_package(Threads REQUIRED) - -# list of examples -set(TOOL_LIST - keyboard_move_tcp -) - -# ===== CONFIGURE ALL TOOLS ===== -foreach(tool ${TOOL_LIST}) - # create executable from source - add_executable(${tool} ${tool}.cpp) - - # link dependencies - target_include_directories(${tool} - PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR}/../../include - ${CMAKE_CURRENT_SOURCE_DIR}/../../thirdparty/Eigen - ) - - target_link_libraries(${tool} - ${CMAKE_CURRENT_SOURCE_DIR}/../../lib/libFlexivRdk.a - anl - Threads::Threads - ) -endforeach() - -# ===== LINK THIRDPARTY IF NEEDED ===== -# RBDyn -find_package(RBDyn REQUIRED) -target_link_libraries(keyboad_move_tcp - RBDyn - RBDynParsers -) diff --git a/example/tool/install_dependencies.sh b/example/tool/install_dependencies.sh deleted file mode 100755 index 162d4970..00000000 --- a/example/tool/install_dependencies.sh +++ /dev/null @@ -1,23 +0,0 @@ -#! /bin/sh -# ========================================================================= -# NOTE: -# These dependencies are only required by the user code in some examples, -# and are NOT required by the RDK library itself. Please refer to Flexiv -# RDK documentation/Getting Started/Build and run examples for more details -# ========================================================================= - -# Dependencies for keyboad_move_tcp.cpp -# ----------------------------------------------------------------------- -# install curl -sudo apt install curl - -# install RBDyn(C++) -curl -1sLf \ - 'https://dl.cloudsmith.io/public/mc-rtc/stable/setup.deb.sh' \ - | sudo -E bash - -sudo apt install librbdyn-dev - -# install boost -sudo apt install libboost-all-dev -# ----------------------------------------------------------------------- diff --git a/example/tool/keyboard_move_tcp.cpp b/example/tool/keyboard_move_tcp.cpp deleted file mode 100644 index 8c02ae4e..00000000 --- a/example/tool/keyboard_move_tcp.cpp +++ /dev/null @@ -1,471 +0,0 @@ -/** - * @example keyboard_move_tcp.cpp - * A tool example to move robot TCP with keyboard. - * @copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. - * @author Flexiv - */ - -#include - -#include -#include -#include -#include - -#include -#include // open -#include // read, close - -#include -#include -#include -#include -#include - -namespace { -// robot DOF -const unsigned int k_robotDofs = 7; - -// size of Cartesian pose vector [position 3x1 + rotation (quaternion) 4x1 ] -const unsigned int k_cartPoseSize = 7; - -// flag whether robot is at home position -bool g_isInitalized = false; - -// flag whether robot is finish going to home position -bool g_isFinished = false; - -// current/target Cartesian-space pose (position + rotation) of robot TCP -std::vector g_currentTcpPose(k_robotDofs); - -// home joint position for ik initial value -std::vector g_homeJointPos(k_robotDofs); - -// last joint position for ik initial value -std::vector g_lastJointPos(k_robotDofs); - -// flag whether user want to exit teleop -bool g_isExit = false; - -// user input string -char g_userInput; - -// user input mutex -std::mutex g_userInputMutex; - -// RBDyn configuration -rbd::parsers::ParserResult g_rbdynURDF; - -// index of joint which is not fixed -std::vector g_rbdIndices; - -// end effecter index -int g_eeIndex; -} - -void printJointInDeg(const std::vector& vec) -{ - for (auto i : vec) { - std::cout << i * 180 / M_PI << " "; - } - std::cout << std::endl; -} - -// get the index of body in RBDyn -int getRbdIndex(const std::string& linkName) -{ - for (size_t i = 0; i < g_rbdynURDF.mb.nrBodies(); ++i) - if (g_rbdynURDF.mb.body(i).name() == linkName) - return i; - return 0; -} - -// initialize RBDyn indices -void initRBDyn(const std::string& urdfFilePath, const std::string& targetName) -{ - g_rbdynURDF = rbd::parsers::from_urdf_file(urdfFilePath); - - g_rbdIndices.clear(); - - for (size_t i = 0; i < g_rbdynURDF.mb.nrJoints(); ++i) { - if (g_rbdynURDF.mb.joint(i).type() != rbd::Joint::Fixed) - g_rbdIndices.push_back(i); - } - - g_eeIndex = getRbdIndex(targetName); -} - -// set targeted flange pose from keyboard reading -void setTargetPose(const char& input) -{ - double speed = 0.0032; //[m]; 1mm per press - // read key at 32Hz, control at 1Hz --> press once, send 30 times - double delta = speed / 32; - if (input == 'w') { - g_currentTcpPose[0] += delta; - } else if (input == 's') { - g_currentTcpPose[0] -= delta; - } else if (input == 'a') { - g_currentTcpPose[1] += delta; - } else if (input == 'd') { - g_currentTcpPose[1] -= delta; - } else if (input == 'q') { - g_currentTcpPose[2] += delta; - } else if (input == 'e') { - g_currentTcpPose[2] -= delta; - } else { - // do nothing; - } -} - -// vector[XYZwxyz] to sva pose -sva::PTransformd vectorToSVATransform(const std::vector& q) -{ - Eigen::Vector3d trans; // x,y,z - trans << q[0], q[1], q[2]; - Eigen::Quaterniond rot(q[3], q[4], q[5], q[6]); // w,x,y,z - sva::PTransformd targetPose(rot, trans); - - return targetPose; -} - -// wrap angle in [-pi,pi] -double wrapAngle(const double& angle) -{ - double wrappedAngle; - if ((angle <= M_PI) && (angle >= -M_PI)) { - wrappedAngle = angle; - } else if (angle < 0.0) { - wrappedAngle = std::fmod(angle - M_PI, 2.0 * M_PI) + M_PI; - } else { - wrappedAngle = std::fmod(angle + M_PI, 2.0 * M_PI) - M_PI; - } - return wrappedAngle; -} - -// solve IK with RBDyn -std::vector solveIK(std::shared_ptr ikSolver, - const std::vector& targetPose, - const std::vector& lastJointPosition) -{ - sva::PTransformd eeTargetPose = vectorToSVATransform(targetPose); - - // read limits from urdf - Eigen::VectorXd q_low = Eigen::VectorXd::Ones(g_rbdIndices.size()); - Eigen::VectorXd q_high = q_low; - - for (size_t i = 0; i < g_rbdIndices.size(); ++i) { - size_t index = g_rbdIndices[i]; - q_low(i) - = g_rbdynURDF.limits.lower[g_rbdynURDF.mb.joint(index).name()][0] - + 3 / 180 * M_PI; - q_high(i) - = g_rbdynURDF.limits.upper[g_rbdynURDF.mb.joint(index).name()][0] - - 3 / 180 * M_PI; - } - - // set IK initial joint value - for (size_t i = 0; i < g_rbdIndices.size(); ++i) { - size_t rbd_index = g_rbdIndices[i]; - g_rbdynURDF.mbc.q[rbd_index][0] = lastJointPosition[i]; - } - - // solve IK - bool ikResult = ikSolver->inverseKinematics( - g_rbdynURDF.mb, g_rbdynURDF.mbc, eeTargetPose); - - // determine whether IK solution is valid - std::vector jointResult(k_robotDofs, 0); - if (ikResult) { - for (size_t i = 0; i < k_robotDofs; ++i) { - size_t index = g_rbdIndices[i]; - jointResult[i] = wrapAngle(g_rbdynURDF.mbc.q[index][0]); - - // add limits - if (jointResult[i] < q_low(i)) - jointResult[i] = q_low(i); - if (jointResult[i] > q_high(i)) - jointResult[i] = q_high(i); - } - } else { - std::cout << "IK NOT SOLVED !! Robot NOT Moving Anymore" << std::endl; - for (size_t i = 0; i < k_robotDofs; ++i) { - jointResult[i] = wrapAngle(lastJointPosition[i]); - } - } - - return jointResult; -} - -void highPriorityPeriodicTask(std::shared_ptr robot, - std::shared_ptr robotStates, - std::shared_ptr ikSolver) -{ - robot->getRobotStates(robotStates.get()); - - // get robot home info - if (!g_isInitalized) { - flexiv::SystemStatus systemStatus; - robot->getSystemStatus(&systemStatus); - - if (robotStates->m_flangePose.size() == k_cartPoseSize) { - g_currentTcpPose = robotStates->m_flangePose; - g_lastJointPos = robotStates->m_q; - } - std::cout << "Initial joint position of robot (in degree) :" - << std::endl; - printJointInDeg(g_lastJointPos); - - // teleop info print in terminal - std::cout << std::endl; - std::cout << "Robot is ready! teleoperate the robot with keyboard: (in " - "World Frame)" - << std::endl; - std::cout << "[w] - move forward - [+X]" << std::endl; - std::cout << "[s] - move backward - [-X]" << std::endl; - std::cout << "[a] - move left - [+Y]" << std::endl; - std::cout << "[d] - move right - [-Y]" << std::endl; - std::cout << "[q] - move up - [+Z]" << std::endl; - std::cout << "[e] - move down - [-Z]" << std::endl; - std::cout << "[Esc] - exit" << std::endl; - - // set mode to joint streaming - robot->setMode(flexiv::MODE_JOINT_POSITION); - g_isInitalized = true; - } - // set target, solve ik and stream joint to robot - else { - // send target - if (!g_isExit) { - char inputBuffer = ' '; - { - std::lock_guard lock(g_userInputMutex); - inputBuffer = g_userInput; - } - - // set targetPose - setTargetPose(inputBuffer); - // solve ik - auto targetPosition - = solveIK(ikSolver, g_currentTcpPose, g_lastJointPos); - - // send joint stream command - std::vector targetVelocity(k_robotDofs, 0); - std::vector targetAcceleration(k_robotDofs, 0); - std::fill(targetVelocity.begin(), targetVelocity.end(), 0.5); - std::fill(targetAcceleration.begin(), targetAcceleration.end(), 0); - robot->streamJointPosition( - targetPosition, targetVelocity, targetAcceleration); - - // update last joint position for IK initial value - g_lastJointPos = robotStates->m_q; - } - // handle exit - else { - robot->setMode(flexiv::MODE_PLAN_EXECUTION); - // wait until mode changed - if (robot->getMode() != flexiv::MODE_PLAN_EXECUTION) { - return; - } - - // if robot hasn't been home - if (!g_isFinished) { - robot->executePlanByName("PLAN-Home"); - - std::cout << "wait robot to home" << std::endl; - flexiv::SystemStatus systemStatus; - // wait until execution finished - do { - robot->getSystemStatus(&systemStatus); - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } while (systemStatus.m_programRunning == true); - - g_isFinished = true; - std::cout << "exit with ctrl-C" << std::endl; - } else - return; - } - } -} - -// read keyboard command through input_event -char readKeyboard(int keysFd, input_event& t) -{ - char inputBuffer; - if (read(keysFd, &t, sizeof(t))) { - if (t.code == KEY_W) { - if (t.value == 1 || t.value == 2) { - inputBuffer = 'w'; - } else if (t.value == 0) { - inputBuffer = ' '; - } - } else if (t.code == KEY_S) { - if (t.value == 1 || t.value == 2) { - inputBuffer = 's'; - } else if (t.value == 0) { - inputBuffer = ' '; - } - } else if (t.code == KEY_A) { - if (t.value == 1 || t.value == 2) { - inputBuffer = 'a'; - } else if (t.value == 0) { - inputBuffer = ' '; - } - } else if (t.code == KEY_D) { - if (t.value == 1 || t.value == 2) { - inputBuffer = 'd'; - } else if (t.value == 0) { - inputBuffer = ' '; - } - } else if (t.code == KEY_Q) { - if (t.value == 1 || t.value == 2) { - inputBuffer = 'q'; - } else if (t.value == 0) { - inputBuffer = ' '; - } - } else if (t.code == KEY_E) { - if (t.value == 1 || t.value == 2) { - inputBuffer = 'e'; - } else if (t.value == 0) { - inputBuffer = ' '; - } - } else if (t.code == KEY_ESC) { - g_isExit = true; - } - } - return inputBuffer; -} - -// keyboard reading task -void readKeyboardTask() -{ - // keyboard device configuration - std::string keyboardDevice = "/dev/input/event3"; - int keysFd; - struct input_event t; - keysFd = open(keyboardDevice.c_str(), O_RDONLY); - if (keysFd <= 0) { - std::cout << "can't open keyboard device!" << std::endl; - exit(-1); - } - std::cout - << "open keyboard device successfully, enter keyboard reading loop" - << std::endl; - - // user input polling - char inputBuffer = ' '; - while (true) { - if (g_isExit) - break; - inputBuffer = readKeyboard(keysFd, t); - { - std::lock_guard lock(g_userInputMutex); - g_userInput = inputBuffer; - } - } - - close(keysFd); - std::cout << "end keyboard" << std::endl; -} - -int main(int argc, char* argv[]) -{ - // Parse Parameters - //============================================================================= - // check if program has 3 arguments - if (argc != 4) { - log.error("Invalid program arguments. Usage: " - "" - << std::endl; - return 0; - } - // IP of the robot server - std::string robotIP = argv[1]; - - // IP of the workstation PC running this program - std::string localIP = argv[2]; - - // type of motion specified by user - std::string urdfFilePath = argv[3]; - - // RBDyn Initialization - //============================================================================= - // parse urdf and set which link to move - initRBDyn(urdfFilePath, "flange"); - - // set rbdyn IK classes - auto ikSolver - = std::make_shared(g_rbdynURDF.mb, g_eeIndex); - - // RDK Initialization - //============================================================================= - // instantiate robot interface - auto robot = std::make_shared(); - - // create data struct for storing robot states - auto robotStates = std::make_shared(); - - // initialize robot interface and connect to the robot server - robot->init(robotIP, localIP); - - // wait for the connection to be established - do { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } while (!robot->isConnected()); - - // enable the robot, make sure the E-stop is released before enabling - if (robot->enable()) { - log.info("Enabling robot ..."); - } - - // wait for the robot to become operational - do { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } while (!robot->isOperational()); - log.info("Robot is now operational"); - - // set mode after robot is operational - robot->setMode(flexiv::MODE_PLAN_EXECUTION); - - // wait for mode to be set - do { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } while (robot->getMode() != flexiv::MODE_PLAN_EXECUTION); - - // Bring Robot To Home - //============================================================================= - robot->executePlanByName("PLAN-Home"); - - flexiv::SystemStatus systemStatus; - // wait execution begin - do { - robot->getSystemStatus(&systemStatus); - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } while (systemStatus.m_programRunning == false); - - // wait execution finished - do { - robot->getSystemStatus(&systemStatus); - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } while (systemStatus.m_programRunning == true); - - // put mode back to IDLE - robot->setMode(flexiv::MODE_IDLE); - do { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } while (robot->getMode() != flexiv::MODE_IDLE); - - // Low-priority Background Tasks - //============================================================================= - // use std::thread for some non-realtime tasks, not joining (non-blocking) - std::thread lowPriorityThread(readKeyboardTask); - - // High-priority Realtime Periodic Task @ 1kHz - //============================================================================= - // this is a blocking method, so all other user-defined background threads - // should be spawned before this - robot->start( - std::bind(highPriorityPeriodicTask, robot, robotStates, ikSolver)); - - return 0; -} \ No newline at end of file diff --git a/include/RobotStates.hpp b/include/RobotStates.hpp index 4734657c..9826ced8 100644 --- a/include/RobotStates.hpp +++ b/include/RobotStates.hpp @@ -168,11 +168,6 @@ struct SystemStatus */ bool m_programRunning; - /** - * @brief If the robot has reached the ready pose - */ - bool m_reachedReady; - /** * @brief If the robot has reached target pose */ @@ -244,4 +239,4 @@ struct PlanInfo } /* namespace flexiv */ -#endif /* FLEXIVRDK_ROBOTSTATES_HPP_ */ \ No newline at end of file +#endif /* FLEXIVRDK_ROBOTSTATES_HPP_ */ diff --git a/lib/libFlexivRdk.a b/lib/libFlexivRdk.a index 3eb95050..e066e5c5 100644 Binary files a/lib/libFlexivRdk.a and b/lib/libFlexivRdk.a differ diff --git a/spec/flexiv_rizon4_kinematics.urdf b/spec/flexiv_rizon4_kinematics.urdf index eb7b01ce..d074c9e5 100644 --- a/spec/flexiv_rizon4_kinematics.urdf +++ b/spec/flexiv_rizon4_kinematics.urdf @@ -6,7 +6,7 @@ - + @@ -27,14 +27,14 @@ - + - + @@ -48,14 +48,14 @@ - + - + diff --git a/test/test_dynamic_engine.cpp b/test/test_dynamic_engine.cpp index a6f1c70a..a64126a9 100644 --- a/test/test_dynamic_engine.cpp +++ b/test/test_dynamic_engine.cpp @@ -113,20 +113,23 @@ void lowPriorityTask() auto deltaG = G - g_G; std::cout << std::fixed << std::setprecision(5); - std::cout << "DIFFERENCE of J between ground truth (MATLAB) and rdk = " + std::cout << "Difference of J between ground truth (MATLAB) and " + "integrated dynamics engine = " << std::endl << deltaJ << std::endl; - std::cout << "norm of delta J: " << deltaJ.norm() << std::endl; + std::cout << "Norm of delta J: " << deltaJ.norm() << '\n' << std::endl; - std::cout << "DIFFERENCE of M between ground truth (MATLAB) and rdk = " + std::cout << "Difference of M between ground truth (MATLAB) and " + "integrated dynamics engine = " << std::endl << deltaM << std::endl; - std::cout << "norm of delta M: " << deltaM.norm() << std::endl; + std::cout << "Norm of delta M: " << deltaM.norm() << '\n' << std::endl; - std::cout << "DIFFERENCE of G between ground truth (MATLAB) and rdk = " + std::cout << "Difference of G between ground truth (MATLAB) and " + "integrated dynamics engine = " << std::endl << deltaG.transpose() << std::endl; - std::cout << "norm of delta G: " << deltaG.norm() << std::endl; + std::cout << "Norm of delta G: " << deltaG.norm() << '\n' << std::endl; } } @@ -186,15 +189,11 @@ int main(int argc, char* argv[]) // Bring Robot To Home //============================================================================= robot->executePlanByName("PLAN-Home"); + // wait for the plan to start + std::this_thread::sleep_for(std::chrono::seconds(1)); + // wait for the execution to finish flexiv::SystemStatus systemStatus; - // wait until execution begin - do { - robot->getSystemStatus(&systemStatus); - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } while (systemStatus.m_programRunning == false); - - // wait until execution finished do { robot->getSystemStatus(&systemStatus); std::this_thread::sleep_for(std::chrono::milliseconds(1)); @@ -212,25 +211,30 @@ int main(int argc, char* argv[]) // load model from robot client robot->loadModel(model.get()); - // J, M, G Hard Code From MATLAB Robotics System Toolbox + // Hard-coded Dynamics Ground Truth from MATLAB //============================================================================= - g_J << 0.11000, 0.07142, 0.03447, -0.36115, -0.06428, 0.12900, 0.00000, - 0.68700, -0.00000, 0.57218, 0.00000, 0.02811, 0.00000, -0.00000, - 0.00000, 0.68700, -0.02893, -0.41778, -0.07660, 0.11000, 0.00000, - 0.00000, -0.00000, 0.64279, 0.00000, 0.76604, -0.00000, 0.00000, - 0.00000, -1.00000, -0.00000, 1.00000, -0.00000, -1.00000, -0.00000, - 1.00000, -0.00000, 0.76604, -0.00000, -0.64279, 0.00000, -1.00000; - - g_M << 2.56854, -0.06665, 1.58254, -0.08723, -0.08345, 0.00977, -0.00108, - -0.06665, 2.87356, 0.06816, -1.14695, -0.13624, 0.17474, -0.00000, - 1.58254, 0.06816, 1.12183, -0.00052, -0.04325, -0.00175, -0.00084, - -0.08723, -1.14695, -0.00052, 1.00788, 0.13052, -0.13536, -0.00007, - -0.08345, -0.13624, -0.04325, 0.13052, 0.03951, -0.02153, 0.00070, - 0.00977, 0.17474, -0.00175, -0.13536, -0.02153, 0.03731, 0.00001, - -0.00108, -0.00000, -0.00084, -0.00007, 0.00070, 0.00001, 0.00111; + // clang-format off + g_J << + 0.110000000000000, 0.078420538028673, 0.034471999940354, -0.368152340866938, -0.064278760968654, 0.136000000000000, -0.000000000000000, + 0.687004857483100, -0.000000000000000, 0.576684003660457, 0.000000000000000, 0.033475407198662, -0.000000000000000, -0.000000000000000, + 0.000000000000000, 0.687004857483100, -0.028925442435894, -0.417782862794537, -0.076604444311898, 0.110000000000000, -0.000000000000000, + 0.000000000000000, -0.000000000000000, 0.642787609686539, 0.000000000000000, 0.766044443118978, -0.000000000000000, 0.000000000000000, + 0, -1.000000000000000, -0.000000000000000, 1.000000000000000, 0.000000000000000, -1.000000000000000, 0.000000000000000, + 1.000000000000000, -0.000000000000000, 0.766044443118978, 0.000000000000000, -0.642787609686539, -0.000000000000000, -1.000000000000000; + + g_M << + 2.480084579964625, -0.066276150278304, 1.520475428405090, -0.082258763257690, -0.082884472612488, 0.008542255000000, -0.000900000000000, + -0.066276150278304, 2.778187401738267, 0.067350373889147, -1.100953424157635, -0.129191018084721, 0.165182543869134, -0.000000000000000, + 1.520475428405090, 0.067350373889147, 1.074177611590570, -0.000410055889147, -0.043572229972025, -0.001968185110853, -0.000689439998807, + -0.082258763257690, -1.100953424157635, -0.000410055889147, 0.964760218577003, 0.123748338084721, -0.125985653288502, 0.000000000000000, + -0.082884472612488, -0.129191018084721, -0.043572229972025, 0.123748338084721, 0.038006882479315, -0.020080821084721, 0.000578508848718, + 0.008542255000000, 0.165182543869134, -0.001968185110853, -0.125985653288502, -0.020080821084721, 0.034608170000000, 0, + -0.000900000000000, -0.000000000000000, -0.000689439998807, 0.000000000000000, 0.000578508848718, 0, 0.000900000000000; // Matlab value is the joint torque to resist gravity - g_G << 0.00000, 48.11561, 1.09735, -19.99695, -2.15003, 2.42853, 0.00000; + g_G << + 0.000000000000000, 46.598931189891374, 1.086347692836129, -19.279904969860052, -2.045058704525489, 2.300886450000000, 0; + // clang-format on // Low-priority Background Tasks //============================================================================= diff --git a/test/test_loop_latency.cpp b/test/test_loop_latency.cpp new file mode 100644 index 00000000..9a2aaa47 --- /dev/null +++ b/test/test_loop_latency.cpp @@ -0,0 +1,142 @@ +/** + * @test test_loop_latency.cpp + * A test to benchmark RDK's loop latency, including communication, computation, + * etc. The workstation PC's serial port is used as reference, and the robot + * server's digital out port is used as test target. + * @copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. + * @author Flexiv + */ + +#include +#include + +#include +#include +#include + +// Standard input/output definitions +#include +// UNIX standard function definitions +#include +// File control definitions +#include +// Error number definitions +#include +// POSIX terminal control definitions +#include + +namespace { +// loop counter +unsigned int g_loopCounter = 0; + +// file descriptor of the serial port +int g_fd = 0; +} + +// callback function for realtime periodic task +void periodicTask(std::shared_ptr robot, flexiv::Log& log) +{ + // send signal at 1Hz + switch (g_loopCounter % 1000) { + case 0: { + log.info( + "Sending benchmark signal to both workstation PC's serial port " + "and robot server's digital out port[0]"); + break; + } + case 1: { + // signal robot server's digital out port + robot->writeDigitalOutput(0, true); + + // signal workstation PC's serial port + auto n = write(g_fd, "0", 1); + if (n < 0) { + log.error("Failed to write to serial port"); + } + + break; + } + case 900: { + // reset digital out after a few seconds + robot->writeDigitalOutput(0, false); + break; + } + default: + break; + } + + g_loopCounter++; +} + +int main(int argc, char* argv[]) +{ + // log object for printing message with timestamp and coloring + flexiv::Log log; + + // Parse Parameters + //============================================================================= + // check if program has 3 arguments + if (argc != 4) { + log.error( + "Invalid program arguments. Usage: " + ""); + log.info( + "Some examples of : /dev/ttyS0 corresponds to " + "COM1, /dev/ttyS1 corresponds to COM2, /dev/ttyUSB0 corresponds to " + "a USB-serial converter"); + return 0; + } + // IP of the robot server + std::string robotIP = argv[1]; + + // IP of the workstation PC running this program + std::string localIP = argv[2]; + + // serial port name + std::string serialPort = argv[3]; + + // RDK Initialization + //============================================================================= + // instantiate robot interface + auto robot = std::make_shared(); + + // initialize robot interface and connect to the robot server + robot->init(robotIP, localIP); + + // wait for the connection to be established + do { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } while (!robot->isConnected()); + + // enable the robot, make sure the E-stop is released before enabling + if (robot->enable()) { + log.info("Enabling robot ..."); + } + + // wait for the robot to become operational + do { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } while (!robot->isOperational()); + log.info("Robot is now operational"); + + // Benchmark Signal + //============================================================================= + // get workstation PC's serial port ready, + g_fd = open( + serialPort.c_str(), O_RDWR | O_NOCTTY | O_NDELAY | O_EXCL | O_CLOEXEC); + + if (g_fd == -1) { + log.error("Unable to open serial port " + serialPort); + } + + // print messages + log.warn("Benchmark signal will be sent every 1 second"); + + // High-priority Realtime Periodic Task @ 1kHz + //============================================================================= + // this is a blocking method, so all other user-defined background + // threads should be spawned before this + robot->start(std::bind(periodicTask, robot, log)); + + return 0; +} diff --git a/test/test_set_tool.cpp b/test/test_set_tool.cpp index a79d0741..70dc2b44 100644 --- a/test/test_set_tool.cpp +++ b/test/test_set_tool.cpp @@ -105,19 +105,17 @@ void lowPriorityTask() auto deltaG = G - g_G; std::cout << std::fixed << std::setprecision(5); - std::cout - << "DIFFERENCE of M between ground truth (MATLAB) and rdk after " - "setTool = " - << std::endl - << deltaM << std::endl; - std::cout << "norm of delta M: " << deltaM.norm() << std::endl; - - std::cout - << "DIFFERENCE of G between ground truth (MATLAB) and rdk after " - "setTool = " - << std::endl - << deltaG.transpose() << std::endl; - std::cout << "norm of delta G: " << deltaG.norm() << std::endl; + std::cout << "Difference of M between ground truth (MATLAB) and " + "integrated dynamics engine after setTool() = " + << std::endl + << deltaM << std::endl; + std::cout << "Norm of delta M: " << deltaM.norm() << '\n' << std::endl; + + std::cout << "Difference of G between ground truth (MATLAB) and " + "integrated dynamics engine after setTool() = " + << std::endl + << deltaG.transpose() << std::endl; + std::cout << "Norm of delta G: " << deltaG.norm() << '\n' << std::endl; } } @@ -177,15 +175,11 @@ int main(int argc, char* argv[]) // Bring Robot To Home //============================================================================= robot->executePlanByName("PLAN-Home"); + // wait for the plan to start + std::this_thread::sleep_for(std::chrono::seconds(1)); + // wait for the execution to finish flexiv::SystemStatus systemStatus; - // wait until execution begin - do { - robot->getSystemStatus(&systemStatus); - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } while (systemStatus.m_programRunning == false); - - // wait until execution finished do { robot->getSystemStatus(&systemStatus); std::this_thread::sleep_for(std::chrono::milliseconds(1)); @@ -205,7 +199,7 @@ int main(int argc, char* argv[]) // Set Tool //============================================================================= - // Robotiq tool info + // artificial tool parameters for verification double mass = 0.9; // com is relative to tcp frame Eigen::Vector3d com = {0.0, 0.0, -0.093}; @@ -217,23 +211,27 @@ int main(int argc, char* argv[]) Eigen::Vector3d tcpPosition = {0.0, 0.0, 0.15}; if (model->setTool(mass, inertia, com, tcpPosition) == false) { - std::cout << "set tool failed! " << std::endl; + log.error("setTool() failed"); return 0; } - std::cout << "set tool successfully" << std::endl; + log.info("setTool() successful"); - // M, G Hard Code From MATLAB Robotics System Toolbox After Set Tool + // Hard-coded Dynamics Ground Truth from MATLAB //============================================================================= - g_M << 3.00477, -0.05394, 1.96283, -0.12863, -0.04579, 0.02819, -0.00164, - -0.05394, 3.31633, 0.05426, -1.45674, -0.19104, 0.26740, -0.00000, - 1.96283, 0.05426, 1.45873, -0.00262, -0.00284, 0.00116, -0.00127, - -0.12863, -1.45674, -0.00262, 1.32548, 0.18352, -0.24987, -0.00007, - -0.04579, -0.19104, -0.00284, 0.18352, 0.05500, -0.03987, 0.00106, - 0.02819, 0.26740, 0.00116, -0.24987, -0.03987, 0.08249, 0.00001, - -0.00164, -0.00000, -0.00127, -0.00007, 0.00106, 0.00001, 0.00168; + // clang-format off + g_M << + 2.916316686749461, -0.052869517013466, 1.903540434220357, -0.124348845003517, -0.041914639740668, 0.027649255000000, -0.001464000000000, + -0.052869517013466, 3.222619358431081, 0.053667041477633, -1.414236317529289, -0.184390078851855, 0.259867572215541, -0.000000000000000, + 1.903540434220357, 0.053667041477633, 1.416023230140298, -0.002724223477633, 0.000093550780077, 0.001155982477633, -0.001121489064726, + -0.124348845003517, -1.414236317529289, -0.002724223477633, 1.287676548627496, 0.177147398851855, -0.244344118313748, 0.000000000000000, + -0.041914639740668, -0.184390078851855, 0.000093550780077, 0.177147398851855, 0.054219756143365, -0.038829881851855, 0.000941041060581, + 0.027649255000000, 0.259867572215541, 0.001155982477633, -0.244344118313748, -0.038829881851855, 0.082171270000000, 0, + -0.001464000000000, -0.000000000000000, -0.001121489064726, 0.000000000000000, 0.000941041060581, 0, 0.001464000000000; // Matlab value is the joint torque to resist gravity - g_G << 0.00000, 54.18118, 0.84197, -23.68555, -2.82637, 3.39972, 0.00000; + g_G << + -0.000000000000001, 52.664497076609663, 0.830964961569619, -22.968509865473024, -2.721399343355234, 3.272076450000000, 0; + // clang-format on // Low-priority Background Tasks //=============================================================================