diff --git a/CMakeLists.txt b/CMakeLists.txt index 9682145c..21293240 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,39 +1,36 @@ -cmake_minimum_required(VERSION 3.4) +cmake_minimum_required(VERSION 3.16.3) # =================================================================== -# PROJECT CONFIG +# PROJECT SETUP # =================================================================== -project(flexiv_rdk VERSION 0.8.0) - -# C++14 required -set(CMAKE_CXX_STANDARD 14) -set(CMAKE_CXX_STANDARD_REQUIRED ON) +project(flexiv_rdk VERSION 0.9.0) # Configure build type if(NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release CACHE STRING "CMake build type" FORCE) endif() +set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Release" "Debug" "RelWithDebInfo") # Set static library message("OS: ${CMAKE_SYSTEM_NAME}") message("Processor: ${CMAKE_SYSTEM_PROCESSOR}") if(${CMAKE_SYSTEM_NAME} MATCHES "Linux") if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "x86_64") - set(RDK_STATIC_LIBRARY "${CMAKE_CURRENT_SOURCE_DIR}/lib/libFlexivRdk.x86_64-linux-gnu.a") + set(RDK_STATIC_LIBRARY "${CMAKE_CURRENT_SOURCE_DIR}/lib/libflexiv_rdk.x86_64-linux-gnu.a") elseif(${CMAKE_SYSTEM_PROCESSOR} MATCHES "aarch64") - set(RDK_STATIC_LIBRARY "${CMAKE_CURRENT_SOURCE_DIR}/lib/libFlexivRdk.aarch64-linux-gnu.a") + set(RDK_STATIC_LIBRARY "${CMAKE_CURRENT_SOURCE_DIR}/lib/libflexiv_rdk.aarch64-linux-gnu.a") else() message(FATAL_ERROR "Linux with ${CMAKE_SYSTEM_PROCESSOR} processor is currently not supported.") endif() elseif(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "arm64") - set(RDK_STATIC_LIBRARY "${CMAKE_CURRENT_SOURCE_DIR}/lib/libFlexivRdk.arm64-darwin.a") + set(RDK_STATIC_LIBRARY "${CMAKE_CURRENT_SOURCE_DIR}/lib/libflexiv_rdk.arm64-darwin.a") else() message(FATAL_ERROR "Mac with ${CMAKE_SYSTEM_PROCESSOR} processor is currently not supported.") endif() elseif(${CMAKE_SYSTEM_NAME} MATCHES "Windows") if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "AMD64") - set(RDK_STATIC_LIBRARY "${CMAKE_CURRENT_SOURCE_DIR}/lib/FlexivRdk.win_amd64.lib") + set(RDK_STATIC_LIBRARY "${CMAKE_CURRENT_SOURCE_DIR}/lib/flexiv_rdk.win_amd64.lib") else() message(FATAL_ERROR "Windows with ${CMAKE_SYSTEM_PROCESSOR} processor is currently not supported.") endif() @@ -42,12 +39,15 @@ endif() # =================================================================== # PROJECT DEPENDENCIES # =================================================================== -# pthread +# Threads set(THREADS_PREFER_PTHREAD_FLAG ON) find_package(Threads REQUIRED) +if(Threads_FOUND) + message(STATUS "Found Threads: HAVE_PTHREAD = ${THREADS_HAVE_PTHREAD_ARG}") +endif() # =================================================================== -# PROJECT LIBRARIES +# CREATE LIBRARY # =================================================================== # Create an INTERFACE library with no source file to compile add_library(${PROJECT_NAME} INTERFACE) @@ -67,10 +67,10 @@ target_link_libraries(${PROJECT_NAME} INTERFACE ) # Use moderate compiler warning option -if(MSVC) - target_compile_options(${PROJECT_NAME} INTERFACE /W1) -else() +if(CMAKE_HOST_UNIX) target_compile_options(${PROJECT_NAME} INTERFACE -Wall -Wextra) +else() + target_compile_options(${PROJECT_NAME} INTERFACE /W1) endif() # Install the INTERFACE library diff --git a/README.md b/README.md index a2755eee..348a8a28 100644 --- a/README.md +++ b/README.md @@ -11,11 +11,11 @@ Flexiv RDK (Robotic Development Kit), a key component of the Flexiv Robotic Soft ## Compatibility Overview -| **Supported OS** | **Supported processor** | **Supported language** | **Required compiler kit** | -| ------------------------------ | ----------------------- | ---------------------- | ------------------------- | -| Linux (Ubuntu 18/20/22 tested) | x86_64, arm64 | C++, Python | build-essential | -| macOS 12 (Monterey) | arm64 | C++, Python | Xcode Command Line Tools | -| Windows 10 | x86_64 | C++, Python | MSVC 14.0+ | +| **Supported OS** | **Supported processor** | **Supported language** | **Required compiler kit** | +| -------------------------- | ----------------------- | ---------------------- | ------------------------- | +| Linux (Ubuntu 20.04/22.04) | x86_64, arm64 | C++, Python | build-essential | +| macOS 12 (Monterey) | arm64 | C++, Python | Xcode Command Line Tools | +| Windows 10 | x86_64 | C++, Python | MSVC 14.0+ | ## Quick Start diff --git a/doc/Doxyfile.in b/doc/Doxyfile.in index 8358f95b..7a20987c 100755 --- a/doc/Doxyfile.in +++ b/doc/Doxyfile.in @@ -38,7 +38,7 @@ PROJECT_NAME = "Flexiv RDK APIs" # could be handy for archiving the generated documentation or if some version # control system is used. -PROJECT_NUMBER = "0.8" +PROJECT_NUMBER = "0.9" # Using the PROJECT_BRIEF tag one can provide an optional one line description # for a project that appears at the top of each page and should give viewer a diff --git a/example/CMakeLists.txt b/example/CMakeLists.txt index 836f81b7..a1c69961 100644 --- a/example/CMakeLists.txt +++ b/example/CMakeLists.txt @@ -1,8 +1,8 @@ - -cmake_minimum_required(VERSION 3.4) +cmake_minimum_required(VERSION 3.16.3) project(flexiv_rdk-examples) -set(CMAKE_CXX_STANDARD 14) +# Show verbose build info +SET(CMAKE_VERBOSE_MAKEFILE ON) message("OS: ${CMAKE_SYSTEM_NAME}") message("Processor: ${CMAKE_SYSTEM_PROCESSOR}") @@ -44,4 +44,8 @@ find_package(flexiv_rdk REQUIRED) foreach(example ${EXAMPLE_LIST}) add_executable(${example} ${example}.cpp) target_link_libraries(${example} flexiv::flexiv_rdk) + # C++14 required + set_target_properties(${example} PROPERTIES + CXX_STANDARD 14 + CXX_STANDARD_REQUIRED ON) endforeach() diff --git a/example/basics1_display_robot_states.cpp b/example/basics1_display_robot_states.cpp index da0466e6..c84d996e 100644 --- a/example/basics1_display_robot_states.cpp +++ b/example/basics1_display_robot_states.cpp @@ -99,14 +99,8 @@ int main(int argc, char* argv[]) robot.enable(); // Wait for the robot to become operational - int secondsWaited = 0; while (!robot.isOperational()) { std::this_thread::sleep_for(std::chrono::seconds(1)); - if (++secondsWaited == 10) { - log.warn( - "Still waiting for robot to become operational, please check that the robot 1) " - "has no fault, 2) is in [Auto (remote)] mode"); - } } log.info("Robot is now operational"); diff --git a/example/basics3_primitive_execution.cpp b/example/basics3_primitive_execution.cpp index 380ea48c..a1619a8a 100644 --- a/example/basics3_primitive_execution.cpp +++ b/example/basics3_primitive_execution.cpp @@ -82,14 +82,8 @@ int main(int argc, char* argv[]) robot.enable(); // Wait for the robot to become operational - int secondsWaited = 0; while (!robot.isOperational()) { std::this_thread::sleep_for(std::chrono::seconds(1)); - if (++secondsWaited == 10) { - log.warn( - "Still waiting for robot to become operational, please check that the robot 1) " - "has no fault, 2) is in [Auto (remote)] mode"); - } } log.info("Robot is now operational"); diff --git a/example/basics4_plan_execution.cpp b/example/basics4_plan_execution.cpp index 5300ba6c..b84d204e 100644 --- a/example/basics4_plan_execution.cpp +++ b/example/basics4_plan_execution.cpp @@ -87,14 +87,8 @@ int main(int argc, char* argv[]) robot.enable(); // Wait for the robot to become operational - int secondsWaited = 0; while (!robot.isOperational()) { std::this_thread::sleep_for(std::chrono::seconds(1)); - if (++secondsWaited == 10) { - log.warn( - "Still waiting for robot to become operational, please check that the robot 1) " - "has no fault, 2) is in [Auto (remote)] mode"); - } } log.info("Robot is now operational"); @@ -136,7 +130,9 @@ int main(int argc, char* argv[]) log.info("Enter plan index to execute:"); int index; std::cin >> index; - robot.executePlan(index); + // Allow the plan to continue its execution even if the RDK program is closed or + // the connection is lost + robot.executePlan(index, true); // Print plan info while the current plan is running while (robot.isBusy()) { @@ -151,7 +147,9 @@ int main(int argc, char* argv[]) log.info("Enter plan name to execute:"); std::string name; std::cin >> name; - robot.executePlan(name); + // Allow the plan to continue its execution even if the RDK program is closed or + // the connection is lost + robot.executePlan(name, true); // Print plan info while the current plan is running while (robot.isBusy()) { diff --git a/example/basics5_zero_force_torque_sensors.cpp b/example/basics5_zero_force_torque_sensors.cpp index 64a60195..7f1897ae 100644 --- a/example/basics5_zero_force_torque_sensors.cpp +++ b/example/basics5_zero_force_torque_sensors.cpp @@ -82,14 +82,8 @@ int main(int argc, char* argv[]) robot.enable(); // Wait for the robot to become operational - int secondsWaited = 0; while (!robot.isOperational()) { std::this_thread::sleep_for(std::chrono::seconds(1)); - if (++secondsWaited == 10) { - log.warn( - "Still waiting for robot to become operational, please check that the robot 1) " - "has no fault, 2) is in [Auto (remote)] mode"); - } } log.info("Robot is now operational"); diff --git a/example/basics6_gripper_control.cpp b/example/basics6_gripper_control.cpp index b5c1d8b7..322c5d47 100644 --- a/example/basics6_gripper_control.cpp +++ b/example/basics6_gripper_control.cpp @@ -106,14 +106,8 @@ int main(int argc, char* argv[]) robot.enable(); // Wait for the robot to become operational - int secondsWaited = 0; while (!robot.isOperational()) { std::this_thread::sleep_for(std::chrono::seconds(1)); - if (++secondsWaited == 10) { - log.warn( - "Still waiting for robot to become operational, please check that the robot 1) " - "has no fault, 2) is in [Auto (remote)] mode"); - } } log.info("Robot is now operational"); diff --git a/example/basics7_auto_recovery.cpp b/example/basics7_auto_recovery.cpp index 1a68da39..0aa1e818 100644 --- a/example/basics7_auto_recovery.cpp +++ b/example/basics7_auto_recovery.cpp @@ -74,14 +74,10 @@ int main(int argc, char* argv[]) // process is done, so just wait long enough for the process to finish std::this_thread::sleep_for(std::chrono::seconds(8)); - // Start auto recovery if the system is in recovery state, the involved joints will start to + // Run auto recovery if the system is in recovery state, the involved joints will start to // move back into allowed position range if (robot.isRecoveryState()) { - robot.startAutoRecovery(); - // Block forever, must reboot the robot and restart user program after recovery is done - while (true) { - std::this_thread::sleep_for(std::chrono::seconds(1)); - } + robot.runAutoRecovery(); } // Otherwise the system is normal, do nothing else { diff --git a/example/intermediate1_realtime_joint_position_control.cpp b/example/intermediate1_realtime_joint_position_control.cpp index b4fe3224..edc8c534 100644 --- a/example/intermediate1_realtime_joint_position_control.cpp +++ b/example/intermediate1_realtime_joint_position_control.cpp @@ -15,6 +15,7 @@ #include #include #include +#include namespace { /** RT loop period [sec] */ @@ -23,6 +24,9 @@ constexpr double k_loopPeriod = 0.001; /** Sine-sweep trajectory amplitude and frequency */ constexpr double k_sineAmp = 0.035; constexpr double k_sineFreq = 0.3; + +/** Atomic signal to stop scheduler tasks */ +std::atomic g_schedStop = {false}; } /** @brief Print tutorial description */ @@ -48,9 +52,8 @@ void printHelp() } /** @brief Callback function for realtime periodic task */ -void periodicTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv::Log& log, - flexiv::RobotStates& robotStates, const std::string& motionType, - const std::vector& initPos) +void periodicTask(flexiv::Robot& robot, flexiv::Log& log, flexiv::RobotStates& robotStates, + const std::string& motionType, const std::vector& initPos) { // Local periodic loop counter static unsigned int loopCounter = 0; @@ -94,7 +97,7 @@ void periodicTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv::Lo } catch (const flexiv::Exception& e) { log.error(e.what()); - scheduler.stop(); + g_schedStop = true; } } @@ -157,14 +160,8 @@ int main(int argc, char* argv[]) robot.enable(); // Wait for the robot to become operational - int secondsWaited = 0; while (!robot.isOperational()) { std::this_thread::sleep_for(std::chrono::seconds(1)); - if (++secondsWaited == 10) { - log.warn( - "Still waiting for robot to become operational, please check that the robot 1) " - "has no fault, 2) is in [Auto (remote)] mode"); - } } log.info("Robot is now operational"); @@ -191,13 +188,22 @@ int main(int argc, char* argv[]) // Create real-time scheduler to run periodic tasks flexiv::Scheduler scheduler; // Add periodic task with 1ms interval and highest applicable priority - scheduler.addTask( - std::bind(periodicTask, std::ref(robot), std::ref(scheduler), std::ref(log), - std::ref(robotStates), std::ref(motionType), std::ref(initPos)), + scheduler.addTask(std::bind(periodicTask, std::ref(robot), std::ref(log), + std::ref(robotStates), std::ref(motionType), std::ref(initPos)), "HP periodic", 1, scheduler.maxPriority()); - // Start all added tasks, this is by default a blocking method + // Start all added tasks scheduler.start(); + // Block and wait for signal to stop scheduler tasks + while (!g_schedStop) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + // Received signal to stop scheduler tasks + scheduler.stop(); + + // Wait a bit for any last-second robot log message to arrive and get printed + std::this_thread::sleep_for(std::chrono::seconds(2)); + } catch (const flexiv::Exception& e) { log.error(e.what()); return 1; diff --git a/example/intermediate2_realtime_joint_torque_control.cpp b/example/intermediate2_realtime_joint_torque_control.cpp index e75abdca..af269cc2 100644 --- a/example/intermediate2_realtime_joint_torque_control.cpp +++ b/example/intermediate2_realtime_joint_torque_control.cpp @@ -17,6 +17,7 @@ #include #include #include +#include namespace { /** RT loop period [sec] */ @@ -29,6 +30,9 @@ const std::vector k_impedanceKd = {80.0, 80.0, 40.0, 40.0, 8.0, 8.0, 8.0 /** Sine-sweep trajectory amplitude and frequency */ constexpr double k_sineAmp = 0.035; constexpr double k_sineFreq = 0.3; + +/** Atomic signal to stop scheduler tasks */ +std::atomic g_schedStop = {false}; } /** @brief Print tutorial description */ @@ -56,9 +60,8 @@ void printHelp() } /** @brief Callback function for realtime periodic task */ -void periodicTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv::Log& log, - flexiv::RobotStates& robotStates, const std::string& motionType, - const std::vector& initPos) +void periodicTask(flexiv::Robot& robot, flexiv::Log& log, flexiv::RobotStates& robotStates, + const std::string& motionType, const std::vector& initPos) { // Local periodic loop counter static unsigned int loopCounter = 0; @@ -107,7 +110,7 @@ void periodicTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv::Lo } catch (const flexiv::Exception& e) { log.error(e.what()); - scheduler.stop(); + g_schedStop = true; } } @@ -170,14 +173,8 @@ int main(int argc, char* argv[]) robot.enable(); // Wait for the robot to become operational - int secondsWaited = 0; while (!robot.isOperational()) { std::this_thread::sleep_for(std::chrono::seconds(1)); - if (++secondsWaited == 10) { - log.warn( - "Still waiting for robot to become operational, please check that the robot 1) " - "has no fault, 2) is in [Auto (remote)] mode"); - } } log.info("Robot is now operational"); @@ -204,13 +201,22 @@ int main(int argc, char* argv[]) // Create real-time scheduler to run periodic tasks flexiv::Scheduler scheduler; // Add periodic task with 1ms interval and highest applicable priority - scheduler.addTask( - std::bind(periodicTask, std::ref(robot), std::ref(scheduler), std::ref(log), - std::ref(robotStates), std::ref(motionType), std::ref(initPos)), + scheduler.addTask(std::bind(periodicTask, std::ref(robot), std::ref(log), + std::ref(robotStates), std::ref(motionType), std::ref(initPos)), "HP periodic", 1, scheduler.maxPriority()); - // Start all added tasks, this is by default a blocking method + // Start all added tasks scheduler.start(); + // Block and wait for signal to stop scheduler tasks + while (!g_schedStop) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + // Received signal to stop scheduler tasks + scheduler.stop(); + + // Wait a bit for any last-second robot log message to arrive and get printed + std::this_thread::sleep_for(std::chrono::seconds(2)); + } catch (const flexiv::Exception& e) { log.error(e.what()); return 1; diff --git a/example/intermediate3_realtime_joint_floating.cpp b/example/intermediate3_realtime_joint_floating.cpp index ffc9001f..11284d4c 100644 --- a/example/intermediate3_realtime_joint_floating.cpp +++ b/example/intermediate3_realtime_joint_floating.cpp @@ -17,10 +17,14 @@ #include #include #include +#include namespace { /** Joint velocity damping gains for floating */ const std::vector k_floatingDamping = {10.0, 10.0, 5.0, 5.0, 1.0, 1.0, 1.0}; + +/** Atomic signal to stop scheduler tasks */ +std::atomic g_schedStop = {false}; } /** @brief Print tutorial description */ @@ -48,8 +52,7 @@ void printHelp() } /** @brief Callback function for realtime periodic task */ -void periodicTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv::Log& log, - flexiv::RobotStates& robotStates) +void periodicTask(flexiv::Robot& robot, flexiv::Log& log, flexiv::RobotStates& robotStates) { try { // Monitor fault on robot server @@ -69,7 +72,7 @@ void periodicTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv::Lo // Add some velocity damping for (size_t i = 0; i < robotDOF; ++i) { - targetTorque[i] = -k_floatingDamping[i] * robotStates.dtheta[i]; + targetTorque[i] += -k_floatingDamping[i] * robotStates.dtheta[i]; } // Send target joint torque to RDK server, enable gravity compensation @@ -78,7 +81,7 @@ void periodicTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv::Lo } catch (const flexiv::Exception& e) { log.error(e.what()); - scheduler.stop(); + g_schedStop = true; } } @@ -131,14 +134,8 @@ int main(int argc, char* argv[]) robot.enable(); // Wait for the robot to become operational - int secondsWaited = 0; while (!robot.isOperational()) { std::this_thread::sleep_for(std::chrono::seconds(1)); - if (++secondsWaited == 10) { - log.warn( - "Still waiting for robot to become operational, please check that the robot 1) " - "has no fault, 2) is in [Auto (remote)] mode"); - } } log.info("Robot is now operational"); @@ -160,12 +157,22 @@ int main(int argc, char* argv[]) // Create real-time scheduler to run periodic tasks flexiv::Scheduler scheduler; // Add periodic task with 1ms interval and highest applicable priority - scheduler.addTask(std::bind(periodicTask, std::ref(robot), std::ref(scheduler), - std::ref(log), std::ref(robotStates)), + scheduler.addTask( + std::bind(periodicTask, std::ref(robot), std::ref(log), std::ref(robotStates)), "HP periodic", 1, scheduler.maxPriority()); - // Start all added tasks, this is by default a blocking method + // Start all added tasks scheduler.start(); + // Block and wait for signal to stop scheduler tasks + while (!g_schedStop) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + // Received signal to stop scheduler tasks + scheduler.stop(); + + // Wait a bit for any last-second robot log message to arrive and get printed + std::this_thread::sleep_for(std::chrono::seconds(2)); + } catch (const flexiv::Exception& e) { log.error(e.what()); return 1; diff --git a/example/intermediate4_realtime_cartesian_pure_motion_control.cpp b/example/intermediate4_realtime_cartesian_pure_motion_control.cpp index ae4a6249..6307cbce 100644 --- a/example/intermediate4_realtime_cartesian_pure_motion_control.cpp +++ b/example/intermediate4_realtime_cartesian_pure_motion_control.cpp @@ -15,6 +15,7 @@ #include #include #include +#include namespace { /** RT loop frequency [Hz] */ @@ -34,6 +35,9 @@ constexpr double k_extForceThreshold = 10.0; /** External joint torque threshold for collision detection, value is only for demo purpose [Nm] */ constexpr double k_extTorqueThreshold = 5.0; + +/** Atomic signal to stop scheduler tasks */ +std::atomic g_schedStop = {false}; } /** @brief Print tutorial description */ @@ -60,12 +64,11 @@ void printHelp() } /** @brief Callback function for realtime periodic task */ -void periodicTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv::Log& log, - flexiv::RobotStates& robotStates, const std::vector& initTcpPose, bool enableHold, - bool enableCollision) +void periodicTask(flexiv::Robot& robot, flexiv::Log& log, flexiv::RobotStates& robotStates, + const std::vector& initPose, bool enableHold, bool enableCollision) { // Local periodic loop counter - static size_t loopCounter = 0; + static uint64_t loopCounter = 0; try { // Monitor fault on robot server @@ -77,13 +80,13 @@ void periodicTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv::Lo // Read robot states robot.getRobotStates(robotStates); - // Initialize target vector - auto targetTcpPose = initTcpPose; + // Initialize target pose to initial pose + auto targetTcpPose = initPose; // Sine-sweep TCP along Y axis if (!enableHold) { targetTcpPose[1] - = initTcpPose[1] + = initPose[1] + k_swingAmp * sin(2 * M_PI * k_swingFreq * loopCounter * k_loopPeriod); } // Otherwise robot TCP will hold at initial pose @@ -97,35 +100,49 @@ void periodicTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv::Lo // Online change preferred joint positions at 3 seconds case (3 * k_loopFreq): { std::vector preferredJntPos - = {-0.938, -1.108, -1.254, 1.464, 1.073, 0.278, -0.658}; + = {0.938, -1.108, -1.254, 1.464, 1.073, 0.278, -0.658}; robot.setNullSpacePosture(preferredJntPos); log.info("Preferred joint positions set to: " + flexiv::utility::vec2Str(preferredJntPos)); } break; - // Online change stiffness to softer at 6 seconds + // Online change stiffness to half of nominal at 6 seconds case (6 * k_loopFreq): { - std::vector newK = {2000, 2000, 2000, 200, 200, 200}; + std::vector newK = robot.info().nominalK; + for (auto& v : newK) { + v *= 0.5; + } robot.setCartesianStiffness(newK); log.info("Cartesian stiffness set to: " + flexiv::utility::vec2Str(newK)); } break; // Online change to another preferred joint positions at 9 seconds case (9 * k_loopFreq): { std::vector preferredJntPos - = {0.938, -1.108, 1.254, 1.464, -1.073, 0.278, 0.658}; + = {-0.938, -1.108, 1.254, 1.464, -1.073, 0.278, 0.658}; robot.setNullSpacePosture(preferredJntPos); log.info("Preferred joint positions set to: " + flexiv::utility::vec2Str(preferredJntPos)); } break; - // Online reset stiffness to original at 12 seconds + // Online reset stiffness to nominal at 12 seconds case (12 * k_loopFreq): { - robot.setCartesianStiffness(); + robot.resetCartesianStiffness(); log.info("Cartesian stiffness is reset"); } break; - // Online reset preferred joint positions at 15 seconds - case (15 * k_loopFreq): { - robot.setNullSpacePosture(); + // Online reset preferred joint positions to nominal at 14 seconds + case (14 * k_loopFreq): { + robot.resetNullSpacePosture(); log.info("Preferred joint positions are reset"); } break; + // Online enable max contact wrench regulation at 16 seconds + case (16 * k_loopFreq): { + std::vector maxWrench = {10.0, 10.0, 10.0, 2.0, 2.0, 2.0}; + robot.setMaxContactWrench(maxWrench); + log.info("Max contact wrench set to: " + flexiv::utility::vec2Str(maxWrench)); + } break; + // Disable max contact wrench regulation at 19 seconds + case (19 * k_loopFreq): { + robot.resetMaxContactWrench(); + log.info("Max contact wrench is reset"); + } break; default: break; } @@ -147,7 +164,7 @@ void periodicTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv::Lo if (collisionDetected) { robot.stop(); log.warn("Collision detected, stopping robot and exit program ..."); - scheduler.stop(); + g_schedStop = true; } } @@ -156,7 +173,7 @@ void periodicTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv::Lo } catch (const flexiv::Exception& e) { log.error(e.what()); - scheduler.stop(); + g_schedStop = true; } } @@ -227,14 +244,8 @@ int main(int argc, char* argv[]) robot.enable(); // Wait for the robot to become operational - int secondsWaited = 0; while (!robot.isOperational()) { std::this_thread::sleep_for(std::chrono::seconds(1)); - if (++secondsWaited == 10) { - log.warn( - "Still waiting for robot to become operational, please check that the robot 1) " - "has no fault, 2) is in [Auto (remote)] mode"); - } } log.info("Robot is now operational"); @@ -248,7 +259,7 @@ int main(int argc, char* argv[]) std::this_thread::sleep_for(std::chrono::seconds(1)); } - // Real-time Cartesian Motion Control + // Zero Force-torque Sensor // ========================================================================================= // IMPORTANT: must zero force/torque sensor offset for accurate force/torque measurement robot.executePrimitive("ZeroFTSensor()"); @@ -263,25 +274,45 @@ int main(int argc, char* argv[]) } log.info("Sensor zeroing complete"); - // Use robot base frame as reference frame for commands - robot.setMode(flexiv::Mode::RT_CARTESIAN_MOTION_FORCE_BASE); + // Configure Motion Control + // ========================================================================================= + // The Cartesian motion force modes do pure motion control out of the box, thus nothing + // needs to be explicitly configured + + // NOTE: motion control always uses robot base frame, while force control can use + // either base or TCP frame as reference frame + + // Start Pure Motion Control + // ========================================================================================= + // Switch to real-time mode for continuous motion control + robot.setMode(flexiv::Mode::RT_CARTESIAN_MOTION_FORCE); - // Set initial TCP pose + // Set initial pose to current TCP pose robot.getRobotStates(robotStates); - auto initTcpPose = robotStates.tcpPose; + auto initPose = robotStates.tcpPose; log.info("Initial TCP pose set to [position 3x1, rotation (quaternion) 4x1]: " - + flexiv::utility::vec2Str(initTcpPose)); + + flexiv::utility::vec2Str(initPose)); // Create real-time scheduler to run periodic tasks flexiv::Scheduler scheduler; // Add periodic task with 1ms interval and highest applicable priority scheduler.addTask( - std::bind(periodicTask, std::ref(robot), std::ref(scheduler), std::ref(log), - std::ref(robotStates), std::ref(initTcpPose), enableHold, enableCollision), + std::bind(periodicTask, std::ref(robot), std::ref(log), std::ref(robotStates), + std::ref(initPose), enableHold, enableCollision), "HP periodic", 1, scheduler.maxPriority()); - // Start all added tasks, this is by default a blocking method + // Start all added tasks scheduler.start(); + // Block and wait for signal to stop scheduler tasks + while (!g_schedStop) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + // Received signal to stop scheduler tasks + scheduler.stop(); + + // Wait a bit for any last-second robot log message to arrive and get printed + std::this_thread::sleep_for(std::chrono::seconds(2)); + } catch (const flexiv::Exception& e) { log.error(e.what()); return 1; diff --git a/example/intermediate5_realtime_cartesian_motion_force_control.cpp b/example/intermediate5_realtime_cartesian_motion_force_control.cpp index 151d3c8f..58e122ab 100644 --- a/example/intermediate5_realtime_cartesian_motion_force_control.cpp +++ b/example/intermediate5_realtime_cartesian_motion_force_control.cpp @@ -1,8 +1,8 @@ /** * @example intermediate5_realtime_cartesian_motion_force_control.cpp - * This tutorial runs real-time Cartesian-space unified motion-force control to apply force along - * Z axis of the chosen reference frame, or to execute a simple polish action along XY plane of the - * chosen reference frame. + * This tutorial runs real-time Cartesian-space unified motion-force control. The Z axis of the + * chosen reference frame will be activated for explicit force control, while the rest axes in the + * same reference frame will stay motion controlled. * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ @@ -16,11 +16,9 @@ #include #include #include +#include namespace { -/** RT loop frequency [Hz] */ -constexpr size_t k_loopFreq = 1000; - /** RT loop period [sec] */ constexpr double k_loopPeriod = 0.001; @@ -32,14 +30,26 @@ constexpr double k_swingFreq = 0.3; /** Pressing force to apply during the unified motion-force control [N] */ constexpr double k_pressingForce = 5.0; + +/** Cartesian linear velocity used to search for contact [m/s] */ +constexpr double k_searchVelocity = 0.02; + +/** Maximum distance to travel when searching for contact [m] */ +constexpr double k_searchDistance = 1.0; + +/** Maximum contact wrench during contact search for soft contact */ +const std::vector k_maxWrenchForContactSearch = {10.0, 10.0, 10.0, 3.0, 3.0, 3.0}; + +/** Atomic signal to stop scheduler tasks */ +std::atomic g_schedStop = {false}; } /** @brief Print tutorial description */ void printDescription() { - std::cout << "This tutorial runs real-time Cartesian-space unified motion-force control to " - "apply force along Z axis of the chosen reference frame, or to execute a simple " - "polish action along XY plane of the chosen reference frame." + std::cout << "This tutorial runs real-time Cartesian-space unified motion-force control. The Z " + "axis of the chosen reference frame will be activated for explicit force control, " + "while the rest axes in the same reference frame will stay motion controlled." << std::endl << std::endl; } @@ -51,25 +61,20 @@ void printHelp() std::cout << "Required arguments: [robot IP] [local IP]" << std::endl; std::cout << " robot IP: address of the robot server" << std::endl; std::cout << " local IP: address of this PC" << std::endl; - std::cout << "Optional arguments: [--hold] [--collision]" << std::endl; - std::cout << " --TCP: use TCP frame as reference frame, otherwise use base frame" << std::endl; - std::cout << " --polish: execute a simple polish action along XY plane, otherwise apply a " - "constant force along Z axis" + std::cout << "Optional arguments: [--TCP] [--polish]" << std::endl; + std::cout << " --TCP: use TCP frame as reference frame for force control, otherwise use base frame" << std::endl; + std::cout << " --polish: run a simple polish motion along XY plane in base frame, otherwise hold robot motion in non-force-control axes" << std::endl << std::endl; // clang-format on } /** Callback function for realtime periodic task */ -void periodicTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv::Log& log, - flexiv::RobotStates& robotStates, const std::vector& initPose, +void periodicTask(flexiv::Robot& robot, flexiv::Log& log, const std::vector& initPose, const std::string frameStr, bool enablePolish) { // Local periodic loop counter - static size_t loopCounter = 0; - - // Local flag indicating the initial contact is made - static bool isContacted = false; + static uint64_t loopCounter = 0; try { // Monitor fault on robot server @@ -78,84 +83,32 @@ void periodicTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv::Lo "periodicTask: Fault occurred on robot server, exiting ..."); } - // Read robot states - robot.getRobotStates(robotStates); - - // Compute norm of sensed external force - Eigen::Vector3d extForce = {robotStates.extWrenchInBase[0], robotStates.extWrenchInBase[1], - robotStates.extWrenchInBase[2]}; - double extForceNorm = extForce.norm(); + // Initialize target pose to initial pose + std::vector targetPose = initPose; - // Set sign of Fz according to reference frame to achieve a "pressing down" behavior + // Set Fz according to reference frame to achieve a "pressing down" behavior double Fz = 0.0; if (frameStr == "BASE") { Fz = -k_pressingForce; } else if (frameStr == "TCP") { Fz = k_pressingForce; } - - // Initialize target vectors - std::vector targetPose = initPose; std::vector targetWrench = {0.0, 0.0, Fz, 0.0, 0.0, 0.0}; - // Search for contact - if (!isContacted) { - // Send both initial pose and wrench commands, the result is force control along Z axis, - // and motion hold along other axes - robot.streamCartesianMotionForce(initPose, targetWrench); - - // Contact is made - if (extForceNorm > k_pressingForce) { - isContacted = true; - log.warn("Contact detected at robot TCP"); - } - - // Skip the rest actions until contact is made - return; + // Apply constant force along Z axis of chosen reference frame, and do a simple polish + // motion along XY plane in robot base frame + if (enablePolish) { + // Create motion command to sine-sweep along Y direction + targetPose[1] = initPose[1] + + k_swingAmp * sin(2 * M_PI * k_swingFreq * loopCounter * k_loopPeriod); + // Command target pose and target wrench + robot.streamCartesianMotionForce(targetPose, targetWrench); } - - // Repeat the following actions in a 20-second cycle: first 15 seconds do unified - // motion-force control, the rest 5 seconds trigger smooth transition to pure motion control - if (loopCounter % (20 * k_loopFreq) == 0) { - // Print info at the beginning of action cycle - if (enablePolish) { - log.info("Executing polish with pressing force [N] = " + std::to_string(Fz)); - } else { - log.info("Applying constant force [N] = " + std::to_string(Fz)); - } - - } else if (loopCounter % (20 * k_loopFreq) == (15 * k_loopFreq)) { - log.info("Disabling force control and transiting smoothly to pure motion control"); - - } else if (loopCounter % (20 * k_loopFreq) == (20 * k_loopFreq - 1)) { - // Reset contact flag at the end of action cycle - isContacted = false; - - } else if (loopCounter % (20 * k_loopFreq) < (15 * k_loopFreq)) { - // Simple polish action along XY plane of chosen reference frame - if (enablePolish) { - // Create motion command to sine-sweep along Y direction - targetPose[1] - = initPose[1] - + k_swingAmp * sin(2 * M_PI * k_swingFreq * loopCounter * k_loopPeriod); - - // Send both target pose and wrench commands, the result is force control along Z - // axis, and motion control along other axes - robot.streamCartesianMotionForce(targetPose, targetWrench); - } - // Apply constant force along Z axis of chosen reference frame - else { - // Send both initial pose and wrench commands, the result is force control along Z - // axis, and motion hold along other axes - robot.streamCartesianMotionForce(initPose, targetWrench); - } - - } else { - // By not passing in targetWrench parameter, the force control will be cancelled and - // transit smoothly back to pure motion control. The previously force-controlled axis - // will be gently pulled toward the motion target currently set for that axis. Here we - // use initPose for example. - robot.streamCartesianMotionForce(initPose); + // Apply constant force along Z axis of chosen reference frame, and hold motions in all + // other axes + else { + // Command initial pose and target wrench + robot.streamCartesianMotionForce(initPose, targetWrench); } // Increment loop counter @@ -163,7 +116,7 @@ void periodicTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv::Lo } catch (const flexiv::Exception& e) { log.error(e.what()); - scheduler.stop(); + g_schedStop = true; } } @@ -188,22 +141,23 @@ int main(int argc, char* argv[]) log.info("Tutorial description:"); printDescription(); - // The reference frame to use, see Robot::streamCartesianMotionForce() for more details + // The reference frame to use for force control, see Robot::setForceControlFrame() for + // more details std::string frameStr = "BASE"; if (flexiv::utility::programArgsExist(argc, argv, "--TCP")) { - log.info("Reference frame used for motion force control: robot TCP frame"); + log.info("Reference frame used for force control: robot TCP frame"); frameStr = "TCP"; } else { - log.info("Reference frame used for motion force control: robot base frame"); + log.info("Reference frame used for force control: robot base frame"); } - // Whether to enable polish action + // Whether to enable polish motion bool enablePolish = false; if (flexiv::utility::programArgsExist(argc, argv, "--polish")) { - log.info("Robot will execute a polish action along XY plane"); + log.info("Robot will run a polish motion along XY plane in robot base frame"); enablePolish = true; } else { - log.info("Robot will apply a constant force along Z axis"); + log.info("Robot will hold its motion in all non-force-controlled axes"); } try { @@ -234,14 +188,8 @@ int main(int argc, char* argv[]) robot.enable(); // Wait for the robot to become operational - int secondsWaited = 0; while (!robot.isOperational()) { std::this_thread::sleep_for(std::chrono::seconds(1)); - if (++secondsWaited == 10) { - log.warn( - "Still waiting for robot to become operational, please check that the robot 1) " - "has no fault, 2) is in [Auto (remote)] mode"); - } } log.info("Robot is now operational"); @@ -255,7 +203,7 @@ int main(int argc, char* argv[]) std::this_thread::sleep_for(std::chrono::seconds(1)); } - // Real-time Cartesian Motion-force Control + // Zero Force-torque Sensor // ========================================================================================= // IMPORTANT: must zero force/torque sensor offset for accurate force/torque measurement robot.executePrimitive("ZeroFTSensor()"); @@ -270,36 +218,106 @@ int main(int argc, char* argv[]) } log.info("Sensor zeroing complete"); - // Get latest robot states + // Search for Contact + // ========================================================================================= + // NOTE: there are several ways to do contact search, such as using primitives, or real-time + // and non-real-time direct motion controls, etc. Here we use non-real-time direct Cartesian + // control for example. + log.info("Searching for contact ..."); + + // Set initial pose to current TCP pose robot.getRobotStates(robotStates); + std::vector initPose = robotStates.tcpPose; + log.info("Initial TCP pose set to [position 3x1, rotation (quaternion) 4x1]: " + + flexiv::utility::vec2Str(initPose)); - // Set control mode and initial pose based on reference frame used - std::vector initPose; - if (frameStr == "BASE") { - robot.setMode(flexiv::Mode::RT_CARTESIAN_MOTION_FORCE_BASE); - // If using base frame, directly read from robot states - initPose = robotStates.tcpPose; - } else if (frameStr == "TCP") { - robot.setMode(flexiv::Mode::RT_CARTESIAN_MOTION_FORCE_TCP); - // If using TCP frame, current TCP is at the reference frame's origin - initPose = {0, 0, 0, 1, 0, 0, 0}; - } else { - throw flexiv::InputException("Invalid reference frame choice"); + // Use non-real-time mode to make the robot go to a set point with its own motion generator + robot.setMode(flexiv::Mode::NRT_CARTESIAN_MOTION_FORCE); + + // Search for contact with max contact wrench set to a small value for making soft contact + robot.setMaxContactWrench(k_maxWrenchForContactSearch); + + // Set target point along -Z direction and expect contact to happen during the travel + auto targetPose = initPose; + targetPose[2] -= k_searchDistance; + + // Send target point to robot to start searching for contact and limit the velocity. Keep + // target wrench 0 at this stage since we are not doing force control yet + robot.sendCartesianMotionForce(targetPose, std::vector(6), k_searchVelocity); + + // Use a while loop to poll robot states and check if a contact is made + bool isContacted = false; + while (!isContacted) { + // Get the latest robot states + robot.getRobotStates(robotStates); + + // Compute norm of sensed external force applied on robot TCP + Eigen::Vector3d extForce = {robotStates.extWrenchInBase[0], + robotStates.extWrenchInBase[1], robotStates.extWrenchInBase[2]}; + + // Contact is considered to be made if sensed TCP force exceeds the threshold + if (extForce.norm() > k_pressingForce) { + isContacted = true; + log.info("Contact detected at robot TCP"); + } + // Check at 1ms interval + std::this_thread::sleep_for(std::chrono::milliseconds(1)); } - log.info("Initial TCP pose set to [position 3x1, rotation (quaternion) 4x1]: " - + flexiv::utility::vec2Str(initPose)); + // Configure Force Control + // ========================================================================================= + // The force control configurations can only be updated when the robot is in IDLE mode + robot.stop(); + + // Set force control reference frame based on program argument. See function doc for more + // details + robot.setForceControlFrame(frameStr); + + // Set which Cartesian axis(s) to activate for force control. See function doc for more + // details. Here we only active Z axis + robot.setForceControlAxis(std::vector {false, false, true, false, false, false}); + + // Uncomment the following line to enable passive force control, otherwise active force + // control is used by default. See function doc for more details + /* robot.setPassiveForceControl(true); */ + + // NOTE: motion control always uses robot base frame, while force control can use + // either base or TCP frame as reference frame + + // Start Unified Motion Force Control + // ========================================================================================= + // Switch to real-time mode for continuous motion force control + robot.setMode(flexiv::Mode::RT_CARTESIAN_MOTION_FORCE); + + // Disable max contact wrench regulation. Need to do this AFTER the force control in Z axis + // is activated (i.e. motion control disabled in Z axis) and the motion force control mode + // is entered, this way the contact force along Z axis is explicitly regulated and will not + // spike after the max contact wrench regulation for motion control is disabled + robot.resetMaxContactWrench(); + + // Update initial pose to current TCP pose + robot.getRobotStates(robotStates); + initPose = robotStates.tcpPose; // Create real-time scheduler to run periodic tasks flexiv::Scheduler scheduler; // Add periodic task with 1ms interval and highest applicable priority - scheduler.addTask( - std::bind(periodicTask, std::ref(robot), std::ref(scheduler), std::ref(log), - std::ref(robotStates), std::ref(initPose), std::ref(frameStr), enablePolish), + scheduler.addTask(std::bind(periodicTask, std::ref(robot), std::ref(log), + std::ref(initPose), std::ref(frameStr), enablePolish), "HP periodic", 1, scheduler.maxPriority()); - // Start all added tasks, this is by default a blocking method + // Start all added tasks scheduler.start(); + // Block and wait for signal to stop scheduler tasks + while (!g_schedStop) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + // Received signal to stop scheduler tasks + scheduler.stop(); + + // Wait a bit for any last-second robot log message to arrive and get printed + std::this_thread::sleep_for(std::chrono::seconds(2)); + } catch (const flexiv::Exception& e) { log.error(e.what()); return 1; diff --git a/example/intermediate6_robot_dynamics.cpp b/example/intermediate6_robot_dynamics.cpp index d64e8dc9..4526e850 100644 --- a/example/intermediate6_robot_dynamics.cpp +++ b/example/intermediate6_robot_dynamics.cpp @@ -10,6 +10,7 @@ #include #include #include +#include #include #include @@ -17,6 +18,12 @@ #include #include #include +#include + +namespace { +/** Atomic signal to stop scheduler tasks */ +std::atomic g_schedStop = {false}; +} /** @brief Print tutorial description */ void printDescription() @@ -40,67 +47,60 @@ void printHelp() } /** @brief Periodic task running at 100 Hz */ -int periodicTask(flexiv::Robot& robot, flexiv::Model& model) +void periodicTask( + flexiv::Robot& robot, flexiv::Log& log, flexiv::RobotStates& robotStates, flexiv::Model& model) { - // Logger for printing message with timestamp and coloring - flexiv::Log log; - - // Data struct for storing robot states - flexiv::RobotStates robotStates; + // Local periodic loop counter + static unsigned int loopCounter = 0; try { - int loopCounter = 0; - while (true) { - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - loopCounter++; + // Monitor fault on robot server + if (robot.isFault()) { + throw flexiv::ServerException( + "periodicTask: Fault occurred on robot server, exiting ..."); + } - // Monitor fault on robot server - if (robot.isFault()) { - throw flexiv::ServerException( - "periodicTask: Fault occurred on robot server, exiting " - "..."); - } + // Mark timer start point + auto tic = std::chrono::high_resolution_clock::now(); - // Mark timer start point - auto tic = std::chrono::high_resolution_clock::now(); - - // Read robot states - robot.getRobotStates(robotStates); - - // Update robot model in dynamics engine - model.updateModel(robotStates.q, robotStates.dtheta); - - // Compute gravity vector - auto g = model.getGravityForce(); - - // Compute mass matrix - auto M = model.getMassMatrix(); - - // Compute Jacobian - auto J = model.getJacobian("flange"); - - // Mark timer end point and get loop time - auto toc = std::chrono::high_resolution_clock::now(); - auto computeTime - = std::chrono::duration_cast(toc - tic).count(); - - // Print at 1Hz - if (loopCounter % 100 == 0) { - // Print time used to compute g, M, J - log.info("Computation time = " + std::to_string(computeTime) + " us"); - std::cout << std::endl; - // Print gravity - std::cout << std::fixed << std::setprecision(5) << "g = " << g.transpose() << "\n" - << std::endl; - // Print mass matrix - std::cout << std::fixed << std::setprecision(5) << "M = " << M << "\n" << std::endl; - // Print Jacobian - std::cout << std::fixed << std::setprecision(5) << "J = " << J << "\n" << std::endl; - } + // Read robot states + robot.getRobotStates(robotStates); + + // Update robot model in dynamics engine + model.update(robotStates.q, robotStates.dtheta); + + // Compute gravity vector + auto g = model.getGravityForce(); + + // Compute mass matrix + auto M = model.getMassMatrix(); + + // Compute Jacobian + auto J = model.getJacobian("flange"); + + // Mark timer end point and get loop time + auto toc = std::chrono::high_resolution_clock::now(); + auto computeTime = std::chrono::duration_cast(toc - tic).count(); + + // Print at 1Hz + if (loopCounter % 100 == 0) { + // Print time used to compute g, M, J + log.info("Computation time = " + std::to_string(computeTime) + " us"); + std::cout << std::endl; + // Print gravity + std::cout << std::fixed << std::setprecision(5) << "g = " << g.transpose() << "\n" + << std::endl; + // Print mass matrix + std::cout << std::fixed << std::setprecision(5) << "M = " << M << "\n" << std::endl; + // Print Jacobian + std::cout << std::fixed << std::setprecision(5) << "J = " << J << "\n" << std::endl; } + + loopCounter++; + } catch (const flexiv::Exception& e) { log.error(e.what()); - return 1; + g_schedStop = true; } } @@ -131,6 +131,9 @@ int main(int argc, char* argv[]) // Instantiate robot interface flexiv::Robot robot(robotIP, localIP); + // Create data struct for storing robot states + flexiv::RobotStates robotStates; + // Clear fault on robot server if any if (robot.isFault()) { log.warn("Fault occurred on robot server, trying to clear ..."); @@ -150,14 +153,8 @@ int main(int argc, char* argv[]) robot.enable(); // Wait for the robot to become operational - int secondsWaited = 0; while (!robot.isOperational()) { std::this_thread::sleep_for(std::chrono::seconds(1)); - if (++secondsWaited == 10) { - log.warn( - "Still waiting for robot to become operational, please check that the robot 1) " - "has no fault, 2) is in [Auto (remote)] mode"); - } } log.info("Robot is now operational"); @@ -176,8 +173,24 @@ int main(int argc, char* argv[]) // Initialize dynamics engine flexiv::Model model(robot); - std::thread periodicTaskThread(periodicTask, std::ref(robot), std::ref(model)); - periodicTaskThread.join(); + // Create real-time scheduler to run periodic tasks + flexiv::Scheduler scheduler; + // Add periodic task with 10 ms interval and highest applicable priority + scheduler.addTask(std::bind(periodicTask, std::ref(robot), std::ref(log), + std::ref(robotStates), std::ref(model)), + "HP periodic", 10, scheduler.maxPriority()); + // Start all added tasks + scheduler.start(); + + // Block and wait for signal to stop scheduler tasks + while (!g_schedStop) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + // Received signal to stop scheduler tasks + scheduler.stop(); + + // Wait a bit for any last-second robot log message to arrive and get printed + std::this_thread::sleep_for(std::chrono::seconds(2)); } catch (const flexiv::Exception& e) { log.error(e.what()); diff --git a/example/intermediate7_teach_by_demonstration.cpp b/example/intermediate7_teach_by_demonstration.cpp index 56c5aaad..ff3d28ad 100644 --- a/example/intermediate7_teach_by_demonstration.cpp +++ b/example/intermediate7_teach_by_demonstration.cpp @@ -95,14 +95,8 @@ int main(int argc, char* argv[]) robot.enable(); // Wait for the robot to become operational - int secondsWaited = 0; while (!robot.isOperational()) { std::this_thread::sleep_for(std::chrono::seconds(1)); - if (++secondsWaited == 10) { - log.warn( - "Still waiting for robot to become operational, please check that the robot 1) " - "has no fault, 2) is in [Auto (remote)] mode"); - } } log.info("Robot is now operational"); diff --git a/example_py/basics1_display_robot_states.py b/example_py/basics1_display_robot_states.py index 8b21826c..ea958702 100644 --- a/example_py/basics1_display_robot_states.py +++ b/example_py/basics1_display_robot_states.py @@ -26,8 +26,10 @@ def print_description(): Print tutorial description. """ - print("This tutorial does the very first thing: check connection with the robot server " - "and print received robot states.") + print( + "This tutorial does the very first thing: check connection with the robot server " + "and print received robot states." + ) print() @@ -46,6 +48,7 @@ def print_robot_states(robot, log): # Print all gripper states, round all float values to 2 decimals log.info("Current robot states:") # fmt: off + print("{") print("q: ", ['%.2f' % i for i in robot_states.q]) print("theta: ", ['%.2f' % i for i in robot_states.theta]) print("dq: ", ['%.2f' % i for i in robot_states.dq]) @@ -62,6 +65,7 @@ def print_robot_states(robot, log): print("FT_sensor_raw_reading: ", ['%.2f' % i for i in robot_states.ftSensorRaw]) print("F_ext_tcp_frame: ", ['%.2f' % i for i in robot_states.extWrenchInTcp]) print("F_ext_base_frame: ", ['%.2f' % i for i in robot_states.extWrenchInBase]) + print("}") # fmt: on time.sleep(1) @@ -71,8 +75,8 @@ def main(): # ============================================================================================== # Parse arguments argparser = argparse.ArgumentParser() - argparser.add_argument('robot_ip', help='IP address of the robot server') - argparser.add_argument('local_ip', help='IP address of this PC') + argparser.add_argument("robot_ip", help="IP address of the robot server") + argparser.add_argument("local_ip", help="IP address of this PC") args = argparser.parse_args() # Define alias @@ -106,22 +110,15 @@ def main(): robot.enable() # Wait for the robot to become operational - seconds_waited = 0 while not robot.isOperational(): time.sleep(1) - seconds_waited += 1 - if seconds_waited == 10: - log.warn( - "Still waiting for robot to become operational, please check that the robot 1) " - "has no fault, 2) is in [Auto (remote)] mode") log.info("Robot is now operational") # Print States # ============================================================================= # Thread for printing robot states - print_thread = threading.Thread( - target=print_robot_states, args=[robot, log]) + print_thread = threading.Thread(target=print_robot_states, args=[robot, log]) print_thread.start() print_thread.join() diff --git a/example_py/basics2_clear_fault.py b/example_py/basics2_clear_fault.py index 8bc8fa86..7706ba5b 100644 --- a/example_py/basics2_clear_fault.py +++ b/example_py/basics2_clear_fault.py @@ -25,8 +25,10 @@ def print_description(): Print tutorial description. """ - print("This tutorial clears minor faults from the robot server if any. Note that " - "critical faults cannot be cleared, see RDK manual for more details.") + print( + "This tutorial clears minor faults from the robot server if any. Note that " + "critical faults cannot be cleared, see RDK manual for more details." + ) print() @@ -35,8 +37,8 @@ def main(): # ============================================================================================== # Parse arguments argparser = argparse.ArgumentParser() - argparser.add_argument('robot_ip', help='IP address of the robot server') - argparser.add_argument('local_ip', help='IP address of this PC') + argparser.add_argument("robot_ip", help="IP address of the robot server") + argparser.add_argument("local_ip", help="IP address of this PC") args = argparser.parse_args() # Define alias diff --git a/example_py/basics3_primitive_execution.py b/example_py/basics3_primitive_execution.py index ee72b2d8..f2b934ba 100644 --- a/example_py/basics3_primitive_execution.py +++ b/example_py/basics3_primitive_execution.py @@ -30,9 +30,11 @@ def print_description(): Print tutorial description. """ - print("This tutorial executes several basic robot primitives (unit skills). For " - "detailed documentation on all available primitives, please see [Flexiv " - "Primitives](https://www.flexiv.com/primitives/).") + print( + "This tutorial executes several basic robot primitives (unit skills). For " + "detailed documentation on all available primitives, please see [Flexiv " + "Primitives](https://www.flexiv.com/primitives/)." + ) print() @@ -41,8 +43,8 @@ def main(): # ============================================================================================== # Parse arguments argparser = argparse.ArgumentParser() - argparser.add_argument('robot_ip', help='IP address of the robot server') - argparser.add_argument('local_ip', help='IP address of this PC') + argparser.add_argument("robot_ip", help="IP address of the robot server") + argparser.add_argument("local_ip", help="IP address of this PC") args = argparser.parse_args() # Define alias @@ -76,14 +78,8 @@ def main(): robot.enable() # Wait for the robot to become operational - seconds_waited = 0 while not robot.isOperational(): time.sleep(1) - seconds_waited += 1 - if seconds_waited == 10: - log.warn( - "Still waiting for robot to become operational, please check that the robot 1) " - "has no fault, 2) is in [Auto (remote)] mode") log.info("Robot is now operational") @@ -102,7 +98,7 @@ def main(): robot.executePrimitive("Home()") # Wait for the primitive to finish - while (robot.isBusy()): + while robot.isBusy(): time.sleep(1) # (2) Move robot joints to target positions @@ -114,7 +110,7 @@ def main(): robot.executePrimitive("MoveJ(target=30 -45 0 90 0 40 30)") # Wait for reached target - while (parse_pt_states(robot.getPrimitiveStates(), "reachedTarget") != "1"): + while parse_pt_states(robot.getPrimitiveStates(), "reachedTarget") != "1": time.sleep(1) # (3) Move robot TCP to a target position in world (base) frame @@ -134,13 +130,14 @@ def main(): # Send command to robot robot.executePrimitive( "MoveL(target=0.65 -0.3 0.2 180 0 180 WORLD WORLD_ORIGIN,waypoints=0.45 0.1 0.2 180 0 " - "180 WORLD WORLD_ORIGIN 0.45 -0.3 0.2 180 0 180 WORLD WORLD_ORIGIN, maxVel=0.2)") + "180 WORLD WORLD_ORIGIN 0.45 -0.3 0.2 180 0 180 WORLD WORLD_ORIGIN, maxVel=0.2)" + ) # The [Move] series primitive won't terminate itself, so we determine if the robot has # reached target location by checking the primitive state "reachedTarget = 1" in the list # of current primitive states, and terminate the current primitive manually by sending a # new primitive command. - while (parse_pt_states(robot.getPrimitiveStates(), "reachedTarget") != "1"): + while parse_pt_states(robot.getPrimitiveStates(), "reachedTarget") != "1": time.sleep(1) # (4) Another MoveL that uses TCP frame @@ -158,12 +155,12 @@ def main(): # Send command to robot. This motion will hold current TCP position and # only do TCP rotation - robot.executePrimitive("MoveL(target=0.0 0.0 0.0 " - + list2str(eulerZYX_deg) - + "TRAJ START)") + robot.executePrimitive( + "MoveL(target=0.0 0.0 0.0 " + list2str(eulerZYX_deg) + "TRAJ START)" + ) # Wait for reached target - while (parse_pt_states(robot.getPrimitiveStates(), "reachedTarget") != "1"): + while parse_pt_states(robot.getPrimitiveStates(), "reachedTarget") != "1": time.sleep(1) # All done, stop robot and put into IDLE mode diff --git a/example_py/basics4_plan_execution.py b/example_py/basics4_plan_execution.py index e8491575..ee322c6e 100644 --- a/example_py/basics4_plan_execution.py +++ b/example_py/basics4_plan_execution.py @@ -27,11 +27,13 @@ def print_description(): Print tutorial description. """ - print("This tutorial executes a plan selected by the user from a list of available " - "plans. A plan is a pre-written script to execute a series of robot primitives " - "with pre-defined transition conditions between 2 adjacent primitives. Users can " - "use Flexiv Elements to compose their own plan and assign to the robot, which " - "will appear in the plan list.") + print( + "This tutorial executes a plan selected by the user from a list of available " + "plans. A plan is a pre-written script to execute a series of robot primitives " + "with pre-defined transition conditions between 2 adjacent primitives. Users can " + "use Flexiv Elements to compose their own plan and assign to the robot, which " + "will appear in the plan list." + ) print() @@ -40,8 +42,8 @@ def main(): # ============================================================================================== # Parse arguments argparser = argparse.ArgumentParser() - argparser.add_argument('robot_ip', help='IP address of the robot server') - argparser.add_argument('local_ip', help='IP address of this PC') + argparser.add_argument("robot_ip", help="IP address of the robot server") + argparser.add_argument("local_ip", help="IP address of this PC") args = argparser.parse_args() # Define alias @@ -76,14 +78,8 @@ def main(): robot.enable() # Wait for the robot to become operational - seconds_waited = 0 while not robot.isOperational(): time.sleep(1) - seconds_waited += 1 - if seconds_waited == 10: - log.warn( - "Still waiting for robot to become operational, please check that the robot 1) " - "has no fault, 2) is in [Auto (remote)] mode") log.info("Robot is now operational") @@ -114,7 +110,9 @@ def main(): # Execute plan by index elif user_input == 2: index = int(input("Enter plan index to execute:\n")) - robot.executePlan(index) + # Allow the plan to continue its execution even if the RDK program is closed or + # the connection is lost + robot.executePlan(index, True) # Print plan info while the current plan is running while robot.isBusy(): @@ -133,7 +131,9 @@ def main(): # Execute plan by name elif user_input == 3: name = str(input("Enter plan name to execute:\n")) - robot.executePlan(name) + # Allow the plan to continue its execution even if the RDK program is closed or + # the connection is lost + robot.executePlan(name, True) # Print plan info while the current plan is running while robot.isBusy(): diff --git a/example_py/basics5_zero_force_torque_sensors.py b/example_py/basics5_zero_force_torque_sensors.py index 26e6cb6e..decc6ff2 100644 --- a/example_py/basics5_zero_force_torque_sensors.py +++ b/example_py/basics5_zero_force_torque_sensors.py @@ -26,9 +26,11 @@ def print_description(): Print tutorial description. """ - print("This tutorial zeros the robot's force and torque sensors, which is a recommended " - "(but not mandatory) step before any operations that require accurate " - "force/torque measurement.") + print( + "This tutorial zeros the robot's force and torque sensors, which is a recommended " + "(but not mandatory) step before any operations that require accurate " + "force/torque measurement." + ) print() @@ -37,8 +39,8 @@ def main(): # ============================================================================================== # Parse arguments argparser = argparse.ArgumentParser() - argparser.add_argument('robot_ip', help='IP address of the robot server') - argparser.add_argument('local_ip', help='IP address of this PC') + argparser.add_argument("robot_ip", help="IP address of the robot server") + argparser.add_argument("local_ip", help="IP address of this PC") args = argparser.parse_args() # Define alias @@ -72,14 +74,8 @@ def main(): robot.enable() # Wait for the robot to become operational - seconds_waited = 0 while not robot.isOperational(): time.sleep(1) - seconds_waited += 1 - if seconds_waited == 10: - log.warn( - "Still waiting for robot to become operational, please check that the robot 1) " - "has no fault, 2) is in [Auto (remote)] mode") log.info("Robot is now operational") @@ -89,8 +85,10 @@ def main(): robot_states = flexivrdk.RobotStates() robot.getRobotStates(robot_states) log.info( - "TCP force and moment reading in base frame BEFORE sensor zeroing: " + - list2str(robot_states.extWrenchInBase) + "[N][Nm]") + "TCP force and moment reading in base frame BEFORE sensor zeroing: " + + list2str(robot_states.extWrenchInBase) + + "[N][Nm]" + ) # Run the "ZeroFTSensor" primitive to automatically zero force and torque sensors robot.setMode(mode.NRT_PRIMITIVE_EXECUTION) @@ -99,18 +97,21 @@ def main(): # WARNING: during the process, the robot must not contact anything, otherwise the result # will be inaccurate and affect following operations log.warn( - "Zeroing force/torque sensors, make sure nothing is in contact with the robot") + "Zeroing force/torque sensors, make sure nothing is in contact with the robot" + ) # Wait for the primitive completion - while (robot.isBusy()): + while robot.isBusy(): time.sleep(1) log.info("Sensor zeroing complete") # Get and print the current TCP force/moment readings robot.getRobotStates(robot_states) log.info( - "TCP force and moment reading in base frame AFTER sensor zeroing: " + - list2str(robot_states.extWrenchInBase) + "[N][Nm]") + "TCP force and moment reading in base frame AFTER sensor zeroing: " + + list2str(robot_states.extWrenchInBase) + + "[N][Nm]" + ) except Exception as e: # Print exception error message diff --git a/example_py/basics6_gripper_control.py b/example_py/basics6_gripper_control.py index 6c4bb1f5..5c44d796 100644 --- a/example_py/basics6_gripper_control.py +++ b/example_py/basics6_gripper_control.py @@ -28,8 +28,10 @@ def print_description(): Print tutorial description. """ - print("This tutorial does position and force control (if available) for grippers " - "supported by Flexiv.") + print( + "This tutorial does position and force control (if available) for grippers " + "supported by Flexiv." + ) print() @@ -41,7 +43,7 @@ def print_gripper_states(gripper, log): # Data struct storing gripper states gripper_states = flexivrdk.GripperStates() - while (not g_is_done): + while not g_is_done: # Get the latest gripper states gripper.getGripperStates(gripper_states) @@ -59,8 +61,8 @@ def main(): # ============================================================================================== # Parse arguments argparser = argparse.ArgumentParser() - argparser.add_argument('robot_ip', help='IP address of the robot server') - argparser.add_argument('local_ip', help='IP address of this PC') + argparser.add_argument("robot_ip", help="IP address of the robot server") + argparser.add_argument("local_ip", help="IP address of this PC") args = argparser.parse_args() # Define alias @@ -94,14 +96,8 @@ def main(): robot.enable() # Wait for the robot to become operational - seconds_waited = 0 while not robot.isOperational(): time.sleep(1) - seconds_waited += 1 - if seconds_waited == 10: - log.warn( - "Still waiting for robot to become operational, please check that the robot 1) " - "has no fault, 2) is in [Auto (remote)] mode") log.info("Robot is now operational") @@ -118,7 +114,8 @@ def main(): # Thread for printing gripper states print_thread = threading.Thread( - target=print_gripper_states, args=[gripper, log]) + target=print_gripper_states, args=[gripper, log] + ) print_thread.start() # Position control diff --git a/example_py/basics7_auto_recovery.py b/example_py/basics7_auto_recovery.py index a93a03ea..7140a6a7 100644 --- a/example_py/basics7_auto_recovery.py +++ b/example_py/basics7_auto_recovery.py @@ -19,17 +19,16 @@ import flexivrdk # fmt: on -# Global flag: whether the gripper control tasks are finished -g_is_done = False - def print_description(): """ Print tutorial description. """ - print("This tutorial runs an automatic recovery process if the robot's safety system is in " - "recovery state. See flexiv::Robot::isRecoveryState() and RDK manual for more details.") + print( + "This tutorial runs an automatic recovery process if the robot's safety system is in " + "recovery state. See flexiv::Robot::isRecoveryState() and RDK manual for more details." + ) print() @@ -38,8 +37,8 @@ def main(): # ============================================================================================== # Parse arguments argparser = argparse.ArgumentParser() - argparser.add_argument('robot_ip', help='IP address of the robot server') - argparser.add_argument('local_ip', help='IP address of this PC') + argparser.add_argument("robot_ip", help="IP address of the robot server") + argparser.add_argument("local_ip", help="IP address of this PC") args = argparser.parse_args() # Define alias @@ -65,18 +64,15 @@ def main(): # process is done, so just wait long enough for the process to finish time.sleep(8) - # Start auto recovery if the system is in recovery state, the involved joints will start to + # Run auto recovery if the system is in recovery state, the involved joints will start to # move back into allowed position range if robot.isRecoveryState(): - robot.startAutoRecovery() - # Block forever, must reboot the robot and restart user program after recovery is done - while True: - time.sleep(1) - + robot.runAutoRecovery() # Otherwise the system is normal, do nothing else: log.info( - "Robot system is not in recovery state, nothing to be done, exiting ...") + "Robot system is not in recovery state, nothing to be done, exiting ..." + ) except Exception as e: # Print exception error message diff --git a/example_py/intermediate1_non_realtime_joint_position_control.py b/example_py/intermediate1_non_realtime_joint_position_control.py index 5df59b82..7010597e 100644 --- a/example_py/intermediate1_non_realtime_joint_position_control.py +++ b/example_py/intermediate1_non_realtime_joint_position_control.py @@ -25,8 +25,10 @@ def print_description(): Print tutorial description. """ - print("This tutorial runs non-real-time joint position control to hold or sine-sweep all " - "robot joints.") + print( + "This tutorial runs non-real-time joint position control to hold or sine-sweep all " + "robot joints." + ) print() @@ -39,16 +41,19 @@ def main(): argparser.add_argument("robot_ip", help="IP address of the robot server") argparser.add_argument("local_ip", help="IP address of this PC") argparser.add_argument( - "frequency", help="command frequency, 20 to 200 [Hz]", type=int) + "frequency", help="command frequency, 1 to 100 [Hz]", type=int + ) # Optional arguments argparser.add_argument( - "--hold", action="store_true", - help="robot holds current joint positions, otherwise do a sine-sweep") + "--hold", + action="store_true", + help="robot holds current joint positions, otherwise do a sine-sweep", + ) args = argparser.parse_args() # Check if arguments are valid frequency = args.frequency - assert (frequency >= 20 and frequency <= 200), "Invalid input" + assert frequency >= 1 and frequency <= 100, "Invalid input" # Define alias robot_states = flexivrdk.RobotStates() @@ -82,14 +87,8 @@ def main(): robot.enable() # Wait for the robot to become operational - seconds_waited = 0 while not robot.isOperational(): time.sleep(1) - seconds_waited += 1 - if seconds_waited == 10: - log.warn( - "Still waiting for robot to become operational, please check that the robot 1) " - "has no fault, 2) is in [Auto (remote)] mode") log.info("Robot is now operational") @@ -99,7 +98,7 @@ def main(): robot.executePrimitive("Home()") # Wait for the primitive to finish - while (robot.isBusy()): + while robot.isBusy(): time.sleep(1) # Non-real-time Joint Position Control @@ -107,10 +106,15 @@ def main(): # Switch to non-real-time joint position control mode robot.setMode(mode.NRT_JOINT_POSITION) - period = 1.0/frequency + period = 1.0 / frequency loop_time = 0 - print("Sending command to robot at", frequency, - "Hz, or", period, "seconds interval") + print( + "Sending command to robot at", + frequency, + "Hz, or", + period, + "seconds interval", + ) # Use current robot joint positions as initial positions robot.getRobotStates(robot_states) @@ -147,13 +151,15 @@ def main(): # Sine-sweep all joints if not args.hold: for i in range(DOF): - target_pos[i] = init_pos[i] + SWING_AMP * \ - math.sin(2 * math.pi * SWING_FREQ * loop_time) + target_pos[i] = init_pos[i] + SWING_AMP * math.sin( + 2 * math.pi * SWING_FREQ * loop_time + ) # Otherwise all joints will hold at initial positions # Send command robot.sendJointPosition( - target_pos, target_vel, target_acc, MAX_VEL, MAX_ACC) + target_pos, target_vel, target_acc, MAX_VEL, MAX_ACC + ) # Increment loop time loop_time += period diff --git a/example_py/intermediate2_non_realtime_cartesian_pure_motion_control.py b/example_py/intermediate2_non_realtime_cartesian_pure_motion_control.py index 10e49f48..5c9c102a 100644 --- a/example_py/intermediate2_non_realtime_cartesian_pure_motion_control.py +++ b/example_py/intermediate2_non_realtime_cartesian_pure_motion_control.py @@ -41,8 +41,10 @@ def print_description(): Print tutorial description. """ - print("This tutorial runs non-real-time Cartesian-space pure motion control to hold or " - "sine-sweep the robot TCP. A simple collision detection is also included.") + print( + "This tutorial runs non-real-time Cartesian-space pure motion control to hold or " + "sine-sweep the robot TCP. A simple collision detection is also included." + ) print() @@ -55,19 +57,24 @@ def main(): argparser.add_argument("robot_ip", help="IP address of the robot server") argparser.add_argument("local_ip", help="IP address of this PC") argparser.add_argument( - "frequency", help="command frequency, 20 to 200 [Hz]", type=int) + "frequency", help="command frequency, 1 to 100 [Hz]", type=int + ) # Optional arguments argparser.add_argument( - "--hold", action="store_true", - help="robot holds current TCP pose, otherwise do a sine-sweep") + "--hold", + action="store_true", + help="robot holds current TCP pose, otherwise do a sine-sweep", + ) argparser.add_argument( - "--collision", action="store_true", - help="enable collision detection, robot will stop upon collision") + "--collision", + action="store_true", + help="enable collision detection, robot will stop upon collision", + ) args = argparser.parse_args() # Check if arguments are valid frequency = args.frequency - assert (frequency >= 20 and frequency <= 200), "Invalid input" + assert frequency >= 1 and frequency <= 100, "Invalid input" # Define alias robot_states = flexivrdk.RobotStates() @@ -112,14 +119,8 @@ def main(): robot.enable() # Wait for the robot to become operational - seconds_waited = 0 while not robot.isOperational(): time.sleep(1) - seconds_waited += 1 - if seconds_waited == 10: - log.warn( - "Still waiting for robot to become operational, please check that the robot 1) " - "has no fault, 2) is in [Auto (remote)] mode") log.info("Robot is now operational") @@ -129,42 +130,58 @@ def main(): robot.executePrimitive("Home()") # Wait for the primitive to finish - while (robot.isBusy()): + while robot.isBusy(): time.sleep(1) - # Non-real-time Cartesian Motion Control - # ========================================================================================== + # Zero Force-torque Sensor + # ========================================================================================= # IMPORTANT: must zero force/torque sensor offset for accurate force/torque measurement robot.executePrimitive("ZeroFTSensor()") # WARNING: during the process, the robot must not contact anything, otherwise the result # will be inaccurate and affect following operations log.warn( - "Zeroing force/torque sensors, make sure nothing is in contact with the robot") + "Zeroing force/torque sensors, make sure nothing is in contact with the robot" + ) # Wait for primitive completion while robot.isBusy(): time.sleep(1) log.info("Sensor zeroing complete") - # Use robot base frame as reference frame for commands - robot.setMode(mode.NRT_CARTESIAN_MOTION_FORCE_BASE) + # Configure Motion Control + # ========================================================================================= + # The Cartesian motion force modes do pure motion control out of the box, thus nothing + # needs to be explicitly configured - # Set loop period - period = 1.0/frequency - loop_counter = 0 - print("Sending command to robot at", frequency, - "Hz, or", period, "seconds interval") + # NOTE: motion control always uses robot base frame, while force control can use + # either base or TCP frame as reference frame + + # Start Pure Motion Control + # ========================================================================================= + # Switch to non-real-time mode for discrete motion control + robot.setMode(mode.NRT_CARTESIAN_MOTION_FORCE) - # Use current robot TCP pose as initial pose + # Set initial pose to current TCP pose robot.getRobotStates(robot_states) init_pose = robot_states.tcpPose.copy() print( "Initial TCP pose set to [position 3x1, rotation (quaternion) 4x1]: ", - init_pose) + init_pose, + ) - # Initialize target vector - target_pose = init_pose.copy() + # Periodic Task + # ========================================================================================= + # Set loop period + period = 1.0 / frequency + loop_counter = 0 + print( + "Sending command to robot at", + frequency, + "Hz, or", + period, + "seconds interval", + ) # Send command periodically at user-specified frequency while True: @@ -178,10 +195,14 @@ def main(): # Read robot states robot.getRobotStates(robot_states) + # Initialize target pose to initial pose + target_pose = init_pose.copy() + # Sine-sweep TCP along Y axis if not args.hold: - target_pose[1] = init_pose[1] + SWING_AMP * \ - math.sin(2 * math.pi * SWING_FREQ * loop_counter * period) + target_pose[1] = init_pose[1] + SWING_AMP * math.sin( + 2 * math.pi * SWING_FREQ * loop_counter * period + ) # Otherwise robot TCP will hold at initial pose # Send command. Calling this method with only target pose input results @@ -191,51 +212,63 @@ def main(): # Do the following operations in sequence for every 20 seconds time_elapsed = loop_counter * period # Online change preferred joint positions at 3 seconds - if (time_elapsed % 20.0 == 3.0): - preferred_jnt_pos = [-0.938, -1.108, - -1.254, 1.464, 1.073, 0.278, -0.658] + if time_elapsed % 20.0 == 3.0: + preferred_jnt_pos = [0.938, -1.108, -1.254, 1.464, 1.073, 0.278, -0.658] robot.setNullSpacePosture(preferred_jnt_pos) log.info("Preferred joint positions set to: ") print(preferred_jnt_pos) - # Online change stiffness to softer at 6 seconds - elif (time_elapsed % 20.0 == 6.0): - new_K = [2000, 2000, 2000, 200, 200, 200] + # Online change stiffness to half of nominal at 6 seconds + elif time_elapsed % 20.0 == 6.0: + new_K = np.multiply(robot.info().nominalK, 0.5) robot.setCartesianStiffness(new_K) log.info("Cartesian stiffness set to: ") print(new_K) # Online change to another preferred joint positions at 9 seconds - elif (time_elapsed % 20.0 == 9.0): - preferred_jnt_pos = [0.938, -1.108, - 1.254, 1.464, -1.073, 0.278, 0.658] + elif time_elapsed % 20.0 == 9.0: + preferred_jnt_pos = [-0.938, -1.108, 1.254, 1.464, -1.073, 0.278, 0.658] robot.setNullSpacePosture(preferred_jnt_pos) log.info("Preferred joint positions set to: ") print(preferred_jnt_pos) - # Online reset stiffness to original at 12 seconds - elif (time_elapsed % 20.0 == 12.0): - robot.setCartesianStiffness() + # Online reset stiffness to nominal at 12 seconds + elif time_elapsed % 20.0 == 12.0: + robot.resetCartesianStiffness() log.info("Cartesian stiffness is reset") - # Online reset preferred joint positions at 15 seconds - elif (time_elapsed % 20.0 == 15.0): - robot.setNullSpacePosture() + # Online reset preferred joint positions to nominal at 14 seconds + elif time_elapsed % 20.0 == 14.0: + robot.resetNullSpacePosture() log.info("Preferred joint positions are reset") + # Online enable max contact wrench regulation at 16 seconds + elif time_elapsed % 20.0 == 16.0: + max_wrench = [10.0, 10.0, 10.0, 2.0, 2.0, 2.0] + robot.setMaxContactWrench(max_wrench) + log.info("Max contact wrench set to: ") + print(max_wrench) + # Disable max contact wrench regulation at 19 seconds + elif time_elapsed % 20.0 == 19.0: + robot.resetMaxContactWrench() + log.info("Max contact wrench is reset") # Simple collision detection: stop robot if collision is detected at # end-effector if args.collision: collision_detected = False - ext_force = np.array([robot_states.extWrenchInBase[0], - robot_states.extWrenchInBase[1], robot_states.extWrenchInBase[2]]) - if (np.linalg.norm(ext_force) > EXT_FORCE_THRESHOLD): + ext_force = np.array( + [ + robot_states.extWrenchInBase[0], + robot_states.extWrenchInBase[1], + robot_states.extWrenchInBase[2], + ] + ) + if np.linalg.norm(ext_force) > EXT_FORCE_THRESHOLD: collision_detected = True for v in robot_states.tauExt: - if (abs(v) > EXT_TORQUE_THRESHOLD): + if abs(v) > EXT_TORQUE_THRESHOLD: collision_detected = True if collision_detected: robot.stop() - log.warn( - "Collision detected, stopping robot and exit program ...") + log.warn("Collision detected, stopping robot and exit program ...") return # Increment loop counter diff --git a/example_py/intermediate3_non_realtime_cartesian_motion_force_control.py b/example_py/intermediate3_non_realtime_cartesian_motion_force_control.py index 242aab7d..ab89776b 100644 --- a/example_py/intermediate3_non_realtime_cartesian_motion_force_control.py +++ b/example_py/intermediate3_non_realtime_cartesian_motion_force_control.py @@ -2,9 +2,9 @@ """intermediate3_non_realtime_cartesian_motion_force_control.py -This tutorial runs non-real-time Cartesian-space unified motion-force control to apply force along -Z axis of the chosen reference frame, or to execute a simple polish action along XY plane of the -chosen reference frame. +This tutorial runs real-time Cartesian-space unified motion-force control. The Z axis of the +chosen reference frame will be activated for explicit force control, while the rest axes in the +same reference frame will stay motion controlled. """ __copyright__ = "Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved." @@ -33,15 +33,26 @@ # Pressing force to apply during the unified motion-force control [N] PRESSING_FORCE = 5.0 +# Cartesian linear velocity used to search for contact [m/s] +SEARCH_VELOCITY = 0.02 + +# Maximum distance to travel when searching for contact [m] +SEARCH_DISTANCE = 1.0 + +# Maximum contact wrench during contact search for soft contact +MAX_WRENCH_FOR_CONTACT_SEARCH = [10.0, 10.0, 10.0, 3.0, 3.0, 3.0] + def print_description(): """ Print tutorial description. """ - print("This tutorial runs non-real-time Cartesian-space unified motion-force control to " - "apply force along Z axis of the chosen reference frame, or to execute a simple " - "polish action along XY plane of the chosen reference frame.") + print( + "This tutorial runs real-time Cartesian-space unified motion-force control. The Z " + "axis of the chosen reference frame will be activated for explicit force control, " + "while the rest axes in the same reference frame will stay motion controlled." + ) print() @@ -54,19 +65,24 @@ def main(): argparser.add_argument("robot_ip", help="IP address of the robot server") argparser.add_argument("local_ip", help="IP address of this PC") argparser.add_argument( - "frequency", help="command frequency, 20 to 200 [Hz]", type=int) + "frequency", help="command frequency, 1 to 100 [Hz]", type=int + ) # Optional arguments argparser.add_argument( - "--TCP", action="store_true", - help="use TCP frame as reference frame, otherwise use base frame") + "--TCP", + action="store_true", + help="use TCP frame as reference frame for force control, otherwise use base frame", + ) argparser.add_argument( - "--polish", action="store_true", - help="execute a simple polish action along XY plane, otherwise apply a constant force along Z axis") + "--polish", + action="store_true", + help="run a simple polish motion along XY plane in base frame, otherwise hold robot motion in non-force-control axes", + ) args = argparser.parse_args() # Check if arguments are valid frequency = args.frequency - assert (frequency >= 20 and frequency <= 200), "Invalid input" + assert frequency >= 1 and frequency <= 100, "Invalid input" # Define alias robot_states = flexivrdk.RobotStates() @@ -80,16 +96,16 @@ def main(): # The reference frame to use, see Robot::sendCartesianMotionForce() for more details frame_str = "BASE" if args.TCP: - log.info("Reference frame used for motion force control: robot TCP frame") + log.info("Reference frame used for force control: robot TCP frame") frame_str = "TCP" else: - log.info("Reference frame used for motion force control: robot base frame") + log.info("Reference frame used for force control: robot base frame") - # Whether to enable polish action + # Whether to enable polish motion if args.polish: - log.info("Robot will execute a polish action along XY plane") + log.info("Robot will run a polish motion along XY plane in robot base frame") else: - log.info("Robot will apply a constant force along Z axis") + log.info("Robot will hold its motion in all non-force-controlled axes") try: # RDK Initialization @@ -114,14 +130,8 @@ def main(): robot.enable() # Wait for the robot to become operational - seconds_waited = 0 while not robot.isOperational(): time.sleep(1) - seconds_waited += 1 - if seconds_waited == 10: - log.warn( - "Still waiting for robot to become operational, please check that the robot 1) " - "has no fault, 2) is in [Auto (remote)] mode") log.info("Robot is now operational") @@ -131,55 +141,131 @@ def main(): robot.executePrimitive("Home()") # Wait for the primitive to finish - while (robot.isBusy()): + while robot.isBusy(): time.sleep(1) - # Non-real-time Cartesian Motion-force Control - # ========================================================================================== + # Zero Force-torque Sensor + # ========================================================================================= # IMPORTANT: must zero force/torque sensor offset for accurate force/torque measurement robot.executePrimitive("ZeroFTSensor()") # WARNING: during the process, the robot must not contact anything, otherwise the result # will be inaccurate and affect following operations log.warn( - "Zeroing force/torque sensors, make sure nothing is in contact with the robot") + "Zeroing force/torque sensors, make sure nothing is in contact with the robot" + ) # Wait for primitive completion while robot.isBusy(): time.sleep(1) log.info("Sensor zeroing complete") - # Get latest robot states + # Search for Contact + # ========================================================================================= + # NOTE: there are several ways to do contact search, such as using primitives, or real-time + # and non-real-time direct motion controls, etc. Here we use non-real-time direct Cartesian + # control for example. + log.info("Searching for contact ...") + + # Set initial pose to current TCP pose robot.getRobotStates(robot_states) + init_pose = robot_states.tcpPose.copy() + print( + "Initial TCP pose set to [position 3x1, rotation (quaternion) 4x1]: ", + init_pose, + ) + + # Use non-real-time mode to make the robot go to a set point with its own motion generator + robot.setMode(mode.NRT_CARTESIAN_MOTION_FORCE) + + # Search for contact with max contact wrench set to a small value for making soft contact + robot.setMaxContactWrench(MAX_WRENCH_FOR_CONTACT_SEARCH) + + # Set target point along -Z direction and expect contact to happen during the travel + target_pose = init_pose.copy() + target_pose[2] -= SEARCH_DISTANCE - # Set control mode and initial pose based on reference frame used - init_pose = [] - if frame_str == "BASE": - robot.setMode(mode.NRT_CARTESIAN_MOTION_FORCE_BASE) - # If using base frame, directly read from robot states - init_pose = robot_states.tcpPose.copy() - elif frame_str == "TCP": - robot.setMode(mode.NRT_CARTESIAN_MOTION_FORCE_TCP) - # If using TCP frame, current TCP is at the reference frame's origin - init_pose = [0, 0, 0, 1, 0, 0, 0] - else: - raise Exception("Invalid reference frame choice") + # Send target point to robot to start searching for contact and limit the velocity. Keep + # target wrench 0 at this stage since we are not doing force control yet + robot.sendCartesianMotionForce(target_pose, [0] * 6, SEARCH_VELOCITY) + + # Use a while loop to poll robot states and check if a contact is made + is_contacted = False + while not is_contacted: + # Get the latest robot states + robot.getRobotStates(robot_states) + # Compute norm of sensed external force applied on robot TCP + ext_force = np.array( + [ + robot_states.extWrenchInBase[0], + robot_states.extWrenchInBase[1], + robot_states.extWrenchInBase[2], + ] + ) + + # Contact is considered to be made if sensed TCP force exceeds the threshold + if np.linalg.norm(ext_force) > PRESSING_FORCE: + is_contacted = True + log.info("Contact detected at robot TCP") + + # Check at 1ms interval + time.sleep(0.001) + + # Configure Force Control + # ========================================================================================= + # The force control configurations can only be updated when the robot is in IDLE mode + robot.stop() + + # Set force control reference frame based on program argument. See function doc for more + # details + robot.setForceControlFrame(frame_str) + + # Set which Cartesian axis(s) to activate for force control. See function doc for more + # details. Here we only active Z axis + robot.setForceControlAxis([False, False, True, False, False, False]) + + # Uncomment the following line to enable passive force control, otherwise active force + # control is used by default. See function doc for more details + # robot.setPassiveForceControl(True) + + # NOTE: motion control always uses robot base frame, while force control can use + # either base or TCP frame as reference frame + + # Start Unified Motion Force Control + # ========================================================================================= + # Switch to non-real-time mode for discrete motion force control + robot.setMode(mode.NRT_CARTESIAN_MOTION_FORCE) + + # Disable max contact wrench regulation. Need to do this AFTER the force control in Z axis + # is activated (i.e. motion control disabled in Z axis) and the motion force control mode + # is entered, this way the contact force along Z axis is explicitly regulated and will not + # spike after the max contact wrench regulation for motion control is disabled + robot.resetMaxContactWrench() + + # Update initial pose to current TCP pose + robot.getRobotStates(robot_states) + init_pose = robot_states.tcpPose.copy() print( "Initial TCP pose set to [position 3x1, rotation (quaternion) 4x1]: ", - init_pose) + init_pose, + ) + # Periodic Task + # ========================================================================================= # Set loop period - period = 1.0/frequency - print("Sending command to robot at", frequency, - "Hz, or", period, "seconds interval") + period = 1.0 / frequency + print( + "Sending command to robot at", + frequency, + "Hz, or", + period, + "seconds interval", + ) # Periodic loop counter loop_counter = 0 - # Flag indicating the initial contact is made - is_contacted = False - # Send command periodically at user-specified frequency while True: # Use sleep to control loop period @@ -189,87 +275,31 @@ def main(): if robot.isFault(): raise Exception("Fault occurred on robot server, exiting ...") - # Read robot states - robot.getRobotStates(robot_states) - - # Compute norm of sensed external force - ext_force = np.array([robot_states.extWrenchInBase[0], - robot_states.extWrenchInBase[1], robot_states.extWrenchInBase[2]]) - ext_force_norm = np.linalg.norm(ext_force) + # Initialize target pose to initial pose + target_pose = init_pose.copy() - # Set sign of Fz according to reference frame to achieve a "pressing down" behavior + # Set Fz according to reference frame to achieve a "pressing down" behavior Fz = 0.0 if frame_str == "BASE": Fz = -PRESSING_FORCE elif frame_str == "TCP": Fz = PRESSING_FORCE - - # Initialize target vectors - target_pose = init_pose.copy() target_wrench = [0.0, 0.0, Fz, 0.0, 0.0, 0.0] - # Search for contact - if not is_contacted: - # Send both initial pose and wrench commands, the result is - # force control along Z axis, and motion hold along other axes - robot.sendCartesianMotionForce(init_pose, target_wrench) - - # Contact is made - if ext_force_norm > PRESSING_FORCE: - is_contacted = True - log.warn("Contact detected at robot TCP") - - # Skip the rest actions until contact is made - continue - - # Repeat the following actions in a 20-second cycle: first 15 seconds - # do unified motion-force control, the rest 5 seconds trigger smooth - # transition to pure motion control - time_elapsed = loop_counter * period - if loop_counter % (20 * frequency) == 0: - # Print info at the beginning of action cycle - if args.polish: - log.info("Executing polish with pressing force [N] = " - + str(Fz)) - else: - log.info("Applying constant force [N] = " + str(Fz)) - - elif loop_counter % (20 * frequency) == (15 * frequency): - # Print info when disabling force control - log.info( - "Disabling force control and transiting smoothly to pure " - "motion control") - - elif loop_counter % (20 * frequency) == (20 * frequency - 1): - # Reset contact flag at the end of action cycle - is_contacted = False - - elif time_elapsed % 20.0 < 15.0: - # Simple polish action along XY plane of chosen reference frame - if args.polish: - # Create motion command to sine-sweep along Y direction - target_pose[1] = init_pose[1] + SWING_AMP * \ - math.sin(2 * math.pi * SWING_FREQ * - loop_counter * period) - - # Send both target pose and wrench commands, the result is - # force control along Z axis, and motion control along other - # axes - robot.sendCartesianMotionForce(target_pose, target_wrench) - - # Apply constant force along Z axis of chosen reference frame - else: - # Send both initial pose and wrench commands, the result is - # force control along Z axis, and motion hold along other axes - robot.sendCartesianMotionForce(init_pose, target_wrench) - + # Apply constant force along Z axis of chosen reference frame, and do a simple polish + # motion along XY plane in robot base frame + if args.polish: + # Create motion command to sine-sweep along Y direction + target_pose[1] = init_pose[1] + SWING_AMP * math.sin( + 2 * math.pi * SWING_FREQ * loop_counter * period + ) + # Command target pose and target wrench + robot.sendCartesianMotionForce(target_pose, target_wrench) + # Apply constant force along Z axis of chosen reference frame, and hold motions in all + # other axes else: - # By not passing in targetWrench parameter, the force control will - # be cancelled and transit smoothly back to pure motion control. - # The previously force-controlled axis will be gently pulled toward - # the motion target currently set for that axis. Here we use - # initPose for example. - robot.sendCartesianMotionForce(init_pose) + # Command initial pose and target wrench + robot.sendCartesianMotionForce(init_pose, target_wrench) # Increment loop counter loop_counter += 1 diff --git a/example_py/intermediate4_teach_by_demonstration.py b/example_py/intermediate4_teach_by_demonstration.py index 4fd100ad..d8a704f2 100644 --- a/example_py/intermediate4_teach_by_demonstration.py +++ b/example_py/intermediate4_teach_by_demonstration.py @@ -33,8 +33,10 @@ def print_description(): Print tutorial description. """ - print("This tutorial shows a demo implementation for teach by demonstration: free-drive the " - "robot and record a series of Cartesian poses, which are then reproduced by the robot.") + print( + "This tutorial shows a demo implementation for teach by demonstration: free-drive the " + "robot and record a series of Cartesian poses, which are then reproduced by the robot." + ) print() @@ -43,8 +45,8 @@ def main(): # ============================================================================================== # Parse arguments argparser = argparse.ArgumentParser() - argparser.add_argument('robot_ip', help='IP address of the robot server') - argparser.add_argument('local_ip', help='IP address of this PC') + argparser.add_argument("robot_ip", help="IP address of the robot server") + argparser.add_argument("local_ip", help="IP address of this PC") args = argparser.parse_args() # Define alias @@ -78,14 +80,8 @@ def main(): robot.enable() # Wait for the robot to become operational - seconds_waited = 0 while not robot.isOperational(): time.sleep(1) - seconds_waited += 1 - if seconds_waited == 10: - log.warn( - "Still waiting for robot to become operational, please check that the robot 1) " - "has no fault, 2) is in [Auto (remote)] mode") log.info("Robot is now operational") @@ -120,7 +116,8 @@ def main(): log.info("New teaching process started") log.warn( - "Hold down the enabling button on the motion bar to activate free drive") + "Hold down the enabling button on the motion bar to activate free drive" + ) # Save current robot pose elif input_buffer == "r": if not robot.isBusy(): @@ -141,14 +138,22 @@ def main(): robot.setMode(mode.NRT_PRIMITIVE_EXECUTION) for i in range(len(saved_poses)): - log.info("Executing pose " + str(i + 1) + "/" - + str(len(saved_poses))) + log.info( + "Executing pose " + str(i + 1) + "/" + str(len(saved_poses)) + ) target_pos = [ - saved_poses[i][0], saved_poses[i][1], saved_poses[i][2]] + saved_poses[i][0], + saved_poses[i][1], + saved_poses[i][2], + ] # Convert quaternion to Euler ZYX required by MoveCompliance primitive - target_quat = [saved_poses[i][3], - saved_poses[i][4], saved_poses[i][5], saved_poses[i][6]] + target_quat = [ + saved_poses[i][3], + saved_poses[i][4], + saved_poses[i][5], + saved_poses[i][6], + ] target_euler_deg = quat2eulerZYX(target_quat, degree=True) robot.executePrimitive( @@ -156,17 +161,23 @@ def main(): + list2str(target_pos) + list2str(target_euler_deg) + "WORLD WORLD_ORIGIN, maxVel=0.3, enableMaxContactWrench=1, maxContactWrench=" - + list2str(MAX_CONTACT_WRENCH) + ")") + + list2str(MAX_CONTACT_WRENCH) + + ")" + ) # Wait for robot to reach target location by checking for "reachedTarget = 1" # in the list of current primitive states - while (parse_pt_states(robot.getPrimitiveStates(), "reachedTarget") != "1"): + while ( + parse_pt_states(robot.getPrimitiveStates(), "reachedTarget") + != "1" + ): time.sleep(1) log.info( "All saved poses are executed, enter 'n' to start a new " "teaching process, 'r' to record more poses, 'e' to repeat " - "execution") + "execution" + ) # Put robot back to free drive robot.setMode(mode.NRT_PLAN_EXECUTION) diff --git a/include/flexiv/Data.hpp b/include/flexiv/Data.hpp index 430fe5b0..2d9b4aa0 100644 --- a/include/flexiv/Data.hpp +++ b/include/flexiv/Data.hpp @@ -12,6 +12,9 @@ namespace flexiv { +/** Number of digital IO ports */ +constexpr size_t k_IOPorts = 16; + /** * @struct RobotInfo * @brief General information of the connected robot. diff --git a/include/flexiv/Gripper.hpp b/include/flexiv/Gripper.hpp index f478aaca..bcc89459 100644 --- a/include/flexiv/Gripper.hpp +++ b/include/flexiv/Gripper.hpp @@ -19,52 +19,55 @@ class Gripper { public: /** - * @brief Create a flexiv::Gripper instance for gripper control. + * @brief [Non-blocking] Create a flexiv::Gripper instance and initialize + * robot gripper control. * @param[in] robot Reference to the instance of flexiv::Robot. - * @throw InitException if the instance failed to initialize. + * @throw InitException if the initialization sequence failed. */ Gripper(const Robot& robot); virtual ~Gripper(); /** - * @brief Grasp with direct force control. Requires the mounted gripper to - * support direct force control. + * @brief [Blocking] Grasp with direct force control. Requires the mounted + * gripper to support direct force control. * @param[in] force Target gripping force. Positive: closing force, * negative: opening force [N]. * @note Applicable operation modes: all modes except IDLE. + * @throw ExecutionException if error occurred during execution. * @warning Target inputs outside the valid range (specified in gripper's * configuration file) will be saturated. - * @throw ExecutionException if error occurred during execution. + * @warning This function blocks until the request is successfully delivered + * to the robot. */ void grasp(double force); /** - * @brief Move the gripper fingers with position control. + * @brief [Blocking] Move the gripper fingers with position control. * @param[in] width Target opening width [m]. * @param[in] velocity Closing/opening velocity, cannot be 0 [m/s]. * @param[in] forceLimit Maximum output force during movement [N]. If not * specified, default force limit of the mounted gripper will be used. * @note Applicable operation modes: all modes except IDLE. + * @throw ExecutionException if error occurred during execution. * @warning Target inputs outside the valid range (specified in gripper's * configuration file) will be saturated. - * @throw ExecutionException if error occurred during execution. + * @warning This function blocks until the request is successfully delivered + * to the robot. */ void move(double width, double velocity, double forceLimit = 0); /** - * @brief Stop the gripper. + * @brief [Blocking] Stop the gripper. * @note Applicable operation modes: all modes. * @throw ExecutionException if error occurred during execution. + * @warning This function blocks until the request is successfully delivered + * to the robot and gripper control is transferred back to plan/primitive. */ void stop(void); /** - * @brief Get current gripper states. + * @brief [Non-blocking] Get current gripper states. * @param[out] output Reference of output data object. - * @throw CommException if there's no response from server. - * @throw ExecutionException if error occurred during execution. - * @warning This method will block until the request-reply operation with - * the server is done. The blocking time varies by communication latency. */ void getGripperStates(GripperStates& output); diff --git a/include/flexiv/Log.hpp b/include/flexiv/Log.hpp index eef406e0..962cb234 100644 --- a/include/flexiv/Log.hpp +++ b/include/flexiv/Log.hpp @@ -22,25 +22,33 @@ class Log virtual ~Log() = default; /** - * @brief Print info message with timestamp and coloring. + * @brief [Non-blocking] Print info message with timestamp and coloring. * @param[in] message Info message. - * @note Color = green. + * @note Color = green font. */ void info(const std::string& message) const; /** - * @brief Print warning message with timestamp and coloring. + * @brief [Non-blocking] Print warning message with timestamp and coloring. * @param[in] message Warning message. - * @note Color = yellow. + * @note Color = yellow font. */ void warn(const std::string& message) const; /** - * @brief Print error message with timestamp and coloring. + * @brief [Non-blocking] Print error message with timestamp and coloring. * @param[in] message Error message. - * @note Color = red. + * @note Color = red font. */ void error(const std::string& message) const; + + /** + * @brief [Non-blocking] Print critical error message with timestamp and + * coloring. + * @param[in] message Critical error message. + * @note Color = white font with red background. + */ + void critical(const std::string& message) const; }; } /* namespace flexiv */ diff --git a/include/flexiv/Mode.hpp b/include/flexiv/Mode.hpp index 55a2624e..b0104316 100644 --- a/include/flexiv/Mode.hpp +++ b/include/flexiv/Mode.hpp @@ -10,7 +10,8 @@ namespace flexiv { /** * @enum Mode - * @brief Operation modes of the robot. + * @brief Robot control modes. Must put the robot into the correct control mode + * before sending any corresponding commands. */ enum Mode { @@ -63,35 +64,19 @@ enum Mode /** * Run real-time Cartesian motion-force control to track continuous commands - * in base frame @ 1kHz. + * @ 1kHz. * @note Real-time (RT) mode * @see flexiv::Robot::streamCartesianMotionForce() */ - RT_CARTESIAN_MOTION_FORCE_BASE, - - /** - * Run real-time Cartesian motion-force control to track continuous commands - * in TCP frame @ 1kHz. - * @note Real-time (RT) mode - * @see flexiv::Robot::streamCartesianMotionForce() - */ - RT_CARTESIAN_MOTION_FORCE_TCP, - - /** - * Run non-real-time Cartesian motion-force control to track discrete - * commands (smoothened by internal motion generator) in base frame. - * @note Non-real-time (NRT) mode - * @see flexiv::Robot::sendCartesianMotionForce() - */ - NRT_CARTESIAN_MOTION_FORCE_BASE, + RT_CARTESIAN_MOTION_FORCE, /** * Run non-real-time Cartesian motion-force control to track discrete - * commands (smoothened by internal motion generator) in TCP frame. + * commands, smoothened by internal motion generator. * @note Non-real-time (NRT) mode * @see flexiv::Robot::sendCartesianMotionForce() */ - NRT_CARTESIAN_MOTION_FORCE_TCP, + NRT_CARTESIAN_MOTION_FORCE, }; } /* namespace flexiv */ diff --git a/include/flexiv/Model.hpp b/include/flexiv/Model.hpp index 94ebc461..6836d7bd 100644 --- a/include/flexiv/Model.hpp +++ b/include/flexiv/Model.hpp @@ -22,33 +22,44 @@ class Model { public: /** - * @brief Create a flexiv::Model instance to get robot dynamics data. + * @brief [Non-blocking] Create a flexiv::Model instance and initialize the + * integrated dynamics engine. * @param[in] robot Reference to the instance of flexiv::Robot. - * @param[in] gravityEarth Earth's gravity vector in base frame. Default - * to \f$ [0.0, 0.0, -9.81]^T \f$. Unit: \f$ [m/s^2] \f$. * @throw InitException if the instance failed to initialize. + * @note The robot mounting position set in Flexiv Elements is automatically + * synced over so that this dynamics engine produces the correct result. */ - Model(const Robot& robot, - const Eigen::Vector3d& gravityEarth = Eigen::Vector3d(0.0, 0.0, -9.81)); + Model(const Robot& robot); virtual ~Model(); /** - * @brief Update robot model using new joint states data. + * @brief [Blocking] Reload robot model using the latest data synced from + * the connected robot. Tool model is also synced. + * @throw CommException if failed to sync model data. + * @throw LogicException if the synced robot model contains invalid data. + * @note Call this function if the robot tool has changed. + * @warning This function blocks until the model data is synced and the + * reloading is finished. + */ + void reload(void); + + /** + * @brief [Non-blocking] Update robot model using new joint states data. * @param[in] positions Current joint positions: \f$ q \in \mathbb{R}^{DOF * \times 1} \f$. Unit: \f$ [rad] \f$. * @param[in] velocities Current joint velocities: \f$ \dot{q} \in * \mathbb{R}^{DOF \times 1} \f$. Unit: \f$ [rad/s] \f$. * @throw InputException if the input vector is of wrong size. */ - void updateModel(const std::vector& positions, + void update(const std::vector& positions, const std::vector& velocities); /** - * @brief Compute and get the Jacobian matrix at the frame of the specified - * link \f$ i \f$, expressed in the base frame. + * @brief [Non-blocking] Compute and get the Jacobian matrix at the frame of + * the specified link \f$ i \f$, expressed in the base frame. * @param[in] linkName Name of the link to get Jacobian for. * @return Jacobian matrix: \f$ ^{0}J_i \in \mathbb{R}^{6 \times DOF} \f$. - * @note Call updateModel() before this method. + * @note Call update() before this method. * @note Available links can be found in the provided URDF. They are * {"base_link", "link1", "link2", "link3", "link4", "link5", "link6", * "link7", "flange"}, plus "tool" if any flange tool is mounted. @@ -57,12 +68,13 @@ class Model const Eigen::MatrixXd getJacobian(const std::string& linkName); /** - * @brief Compute and get the time derivative of Jacobian matrix at the - * frame of the specified link \f$ i \f$, expressed in the base frame. + * @brief [Non-blocking] Compute and get the time derivative of Jacobian + * matrix at the frame of the specified link \f$ i \f$, expressed in the + * base frame. * @param[in] linkName Name of the link to get Jacobian derivative for. * @return Time derivative of Jacobian matrix: \f$ ^{0}\dot{J_i} \in * \mathbb{R}^{6 \times DOF} \f$. - * @note Call updateModel() before this method. + * @note Call update() before this method. * @note Available links can be found in the provided URDF. They are * {"base_link", "link1", "link2", "link3", "link4", "link5", "link6", * "link7", "flange"}, plus "tool" if any flange tool is mounted. @@ -71,38 +83,38 @@ class Model const Eigen::MatrixXd getJacobianDot(const std::string& linkName); /** - * @brief Compute and get the mass matrix for the generalized coordinates, - * i.e. joint space. + * @brief [Non-blocking] Compute and get the mass matrix for the generalized + * coordinates, i.e. joint space. * @return Symmetric positive definite mass matrix: \f$ M(q) \in * \mathbb{S}^{DOF \times DOF}_{++} \f$. Unit: \f$ [kgm^2] \f$. - * @note Call updateModel() before this method. + * @note Call update() before this method. */ const Eigen::MatrixXd getMassMatrix(); /** - * @brief Compute and get the Coriolis/centripetal matrix for the - * generalized coordinates, i.e. joint space. + * @brief [Non-blocking] Compute and get the Coriolis/centripetal matrix for + * the generalized coordinates, i.e. joint space. * @return Coriolis/centripetal matrix: \f$ C(q,\dot{q}) \in \mathbb{R}^{DOF * \times DOF} \f$. - * @note Call updateModel() before this method. + * @note Call update() before this method. */ const Eigen::MatrixXd getCoriolisMatrix(); /** - * @brief Compute and get the gravity force vector for the generalized - * coordinates, i.e. joint space. + * @brief [Non-blocking] Compute and get the gravity force vector for the + * generalized coordinates, i.e. joint space. * @return Gravity force vector: \f$ g(q) \in \mathbb{R}^{DOF \times 1} \f$. * Unit: \f$ [Nm] \f$. - * @note Call updateModel() before this method. + * @note Call update() before this method. */ const Eigen::VectorXd getGravityForce(); /** - * @brief Compute and get the Coriolis force vector for the generalized - * coordinates, i.e. joint space. + * @brief [Non-blocking] Compute and get the Coriolis force vector for the + * generalized coordinates, i.e. joint space. * @return Coriolis force vector: \f$ c(q,\dot{q}) \in \mathbb{R}^{DOF * \times 1} \f$. Unit: \f$ [Nm] \f$. - * @note Call updateModel() before this method. + * @note Call update() before this method. */ const Eigen::VectorXd getCoriolisForce(); diff --git a/include/flexiv/Robot.hpp b/include/flexiv/Robot.hpp index be22b6ce..c2f4e5e4 100644 --- a/include/flexiv/Robot.hpp +++ b/include/flexiv/Robot.hpp @@ -9,6 +9,7 @@ #include "Data.hpp" #include "Mode.hpp" +#include #include #include #include @@ -17,89 +18,105 @@ namespace flexiv { /** * @class Robot - * @brief Main interface with the robot, containing system, control, motion, and - * IO methods. This interface is also responsible for communication. + * @brief Main interface with the robot, containing several function categories + * and background services. */ class Robot { public: /** - * @brief Create a flexiv::Robot instance as the main robot interface. RDK - * services will start and connection with robot server will be established. - * @param[in] serverIP IP address of the robot server (remote). + * @brief [Blocking] Create a flexiv::Robot instance as the main robot + * interface. RDK services will initialize and connection with the robot + * will be established. + * @param[in] serverIP IP address of the robot to connect (remote). * @param[in] localIP IP address of the workstation PC (local). * @throw InitException if the instance failed to initialize. * @throw CompatibilityException if the RDK library version is incompatible - * with robot server. - * @throw CommException if the connection with robot server failed to - * establish. + * with the connected robot. + * @throw CommException if failed to establish connection with the robot. + * @warning This constructor blocks until the initialization sequence is + * successfully finished and connection with the robot is established. */ Robot(const std::string& serverIP, const std::string& localIP); virtual ~Robot(); /** - * @brief Access general information of the robot. + * @brief [Non-blocking] Access general information of the robot. * @return RobotInfo struct. */ const RobotInfo info(void); //============================= SYSTEM CONTROL ============================= /** - * @brief Enable the robot, if E-stop is released and there's no fault, the - * robot will release brakes, and becomes operational a few seconds later. + * @brief [Blocking] Enable the robot, if E-stop is released and there's no + * fault, the robot will release brakes, and becomes operational a few + * seconds later. * @throw ExecutionException if error occurred during execution. - * @throw CommException if connection with robot server is lost. + * @throw CommException if the robot is not connected. + * @warning This function blocks until the request is successfully delivered + * to the robot. */ void enable(void); /** - * @brief Stop the robot and transit robot mode to Idle. + * @brief [Blocking] Stop the robot and transit robot mode to Idle. * @throw ExecutionException if error occurred during execution. + * @warning This function blocks until the robot comes to a complete stop. */ void stop(void); /** - * @brief Check if the robot is normally operational, which requires the - * following conditions to be met: enabled, brakes fully released, in auto - * mode, no fault, and not in reduced state. + * @brief [Non-blocking] Check if the robot is normally operational, which + * requires the following conditions to be met: enabled, brakes fully + * released, in auto mode, no fault, and not in reduced state. + * @param[in] verbose Whether to print warning message indicating why the + * robot is not operational when this function returns false. + * @return True: operational, false: not operational. * @warning The robot won't execute any user command until it becomes * normally operational. - * @return True: operational, false: not operational. */ - bool isOperational(void) const; + bool isOperational(bool verbose = true) const; /** - * @brief Check if the robot is currently executing a task. This includes - * any user commanded operations that requires the robot to execute. For - * example, plans, primitives, Cartesian and joint motions, etc. + * @brief [Non-blocking] Check if the robot is currently executing a task. + * This includes any user commanded operations that requires the robot to + * execute. For example, plans, primitives, Cartesian and joint motions, + * etc. + * @return True: busy, false: idle. * @warning Some exceptions exist for primitives, see executePrimitive() * warning for more details. - * @return True: busy, false: idle. */ bool isBusy(void) const; /** - * @brief Check if the connection with the robot server is established. + * @brief [Non-blocking] Check if the connection with the robot is + * established. * @return True: connected, false: disconnected. */ bool isConnected(void) const; /** - * @brief Check if the robot is in fault state. + * @brief [Non-blocking] Check if the robot is in fault state. * @return True: robot has fault, false: robot normal. */ bool isFault(void) const; /** - * @brief Check if the Emergency Stop is released. - * @note True: E-stop released, false: E-stop pressed + * @brief [Non-blocking] Check if the Emergency Stop is released. + * @return True: released, false: pressed. */ bool isEstopReleased(void) const; /** - * @brief Check if the robot system is in recovery state. + * @brief [Non-blocking] Check if the enabling button is pressed. + * @return True: pressed, false: released. + */ + bool isEnablingButtonPressed(void) const; + + /** + * @brief [Non-blocking] Check if the robot system is in recovery state. * @return True: in recovery state, false: not in recovery state. - * @note Use startAutoRecovery() to carry out recovery operation. + * @note Use runAutoRecovery() to carry out automatic recovery operation. * @par Recovery state * The robot system will enter recovery state if it needs to recover * from joint position limit violation (a critical system fault that @@ -111,215 +128,224 @@ class Robot bool isRecoveryState(void) const; /** - * @brief Try establishing connection with the robot server. + * @brief [Blocking] Try establishing connection with the robot. * @throw CommException if failed to establish connection. + * @warning This function blocks until the reconnection is finished. */ void connect(void); /** - * @brief Disconnect with the robot server. + * @brief [Blocking] Disconnect with the robot. * @throw ExecutionException if error occurred during execution. + * @warning This function blocks until the disconnection is finished. */ void disconnect(void); /** - * @brief Clear minor fault of the robot. + * @brief [Blocking] Clear minor fault of the robot. * @throw ExecutionException if error occurred during execution. + * @warning This function blocks until the request is successfully delivered + * to the robot. */ void clearFault(void); /** - * @brief Set a new operation mode to the robot and wait until the mode - * transition is finished. + * @brief [Blocking] Set a new control mode to the robot and wait until + * the mode transition is finished. * @param[in] mode flexiv::Mode enum. - * @warning To avoid unexpected behavior, it's recommended to call stop() - * and check if the robot has come to a complete stop using isStopped() - * before switching mode. * @throw InputException if requested mode is invalid. - * @throw LogicException if robot is in an unknown operation mode. + * @throw LogicException if robot is in an unknown control mode. * @throw ServerException if robot is not operational. * @throw ExecutionException if failed to transit the robot into specified - * operation mode after several attempts. - * @warning This method will block until the robot has successfully - * transited into the specified operation mode. + * control mode after several attempts. + * @warning If the robot is still moving when this function is called, it + * will automatically stop then make the mode transition. + * @warning This function blocks until the robot has successfully transited + * into the specified control mode. */ void setMode(Mode mode); /** - * @brief Get the current operation mode of the robot. + * @brief [Non-blocking] Get the current control mode of the robot. * @return flexiv::Mode enum. */ Mode getMode(void) const; - //============================ ROBOT OPERATION ============================= + //============================ ROBOT OPERATIONS ============================ /** - * @brief Get robot states like joint position, velocity, torque, TCP - * pose, velocity, etc. + * @brief [Non-blocking] Get the latest robot states. * @param[out] output Reference of output data object. - * @note Call this method periodically to get the latest states. + * @note Call this function periodically to keep the output data object up + * to date. */ void getRobotStates(RobotStates& output); /** - * @brief Execute a plan by specifying its index. + * @brief [Blocking] Execute a plan by specifying its index. * @param[in] index Index of the plan to execute, can be obtained via * getPlanNameList(). - * @note Applicable operation mode: NRT_PLAN_EXECUTION. + * @param[in] continueExec Whether to continue executing the plan when + * the RDK program is closed or the connection is lost. * @throw InputException if index is invalid. - * @throw LogicException if robot is not in the correct operation mode. + * @throw LogicException if robot is not in the correct control mode. * @throw ExecutionException if error occurred during execution. + * @note Applicable control mode: NRT_PLAN_EXECUTION. * @note isBusy() can be used to check if a plan task has finished. - * @warning This method will block for 50 ms for the non-real-time command - * to be transmitted and fully processed by the robot server. + * @warning This function blocks until the request is successfully delivered + * to the robot and fully processed. */ - void executePlan(unsigned int index); + void executePlan(unsigned int index, bool continueExec = false); /** - * @brief Execute a plan by specifying its name. + * @brief [Blocking] Execute a plan by specifying its name. * @param[in] name Name of the plan to execute, can be obtained via * getPlanNameList(). - * @note Applicable operation mode: NRT_PLAN_EXECUTION. - * @throw LogicException if robot is not in the correct operation mode. + * @param[in] continueExec Whether to continue executing the plan when + * the RDK program is closed or the connection is lost. + * @throw LogicException if robot is not in the correct control mode. * @throw ExecutionException if error occurred during execution. + * @note Applicable control mode: NRT_PLAN_EXECUTION. * @note isBusy() can be used to check if a plan task has finished. - * @warning This method will block for 50 ms for the non-real-time command - * to be transmitted and fully processed by the robot server. + * @warning This function blocks until the request is successfully delivered + * to the robot and fully processed. */ - void executePlan(const std::string& name); + void executePlan(const std::string& name, bool continueExec = false); /** - * @brief Pause or resume the execution of the current plan. + * @brief [Blocking] Pause or resume the execution of the current plan. * @param[in] pause True: pause plan, false: resume plan. - * @note Applicable operation mode: NRT_PLAN_EXECUTION. - * @throw LogicException if robot is not in the correct operation mode. + * @throw LogicException if robot is not in the correct control mode. * @throw ExecutionException if error occurred during execution. - * @warning This method will block for 50 ms for the non-real-time command - * to be transmitted and fully processed by the robot server. + * @note Applicable control mode: NRT_PLAN_EXECUTION. + * @warning This function blocks until the request is successfully delivered + * to the robot and fully processed. */ void pausePlan(bool pause); /** - * @brief Get a list of all available plans. + * @brief [Blocking] Get a list of all available plans from the robot. * @return Available plans in the format of a string list. - * @throw CommException if there's no response from server. + * @throw CommException if there's no response from the robot. * @throw ExecutionException if error occurred during execution. - * @warning This method will block until the request-reply operation with - * the server is done. The blocking time varies by communication latency. + * @warning This function blocks until the reply from the robot is received. */ std::vector getPlanNameList(void) const; /** - * @brief Get detailed information about the currently running plan. - * Contains information like plan name, primitive name, node name, node - * path, node path time period, etc. + * @brief [Blocking] Get detailed information about the currently running + * plan. Contains information like plan name, primitive name, node name, + * node path, node path time period, etc. * @param[out] output Reference of output data object. - * @throw CommException if there's no response from server. + * @throw CommException if there's no response from the robot. * @throw ExecutionException if error occurred during execution. - * @warning This method will block until the request-reply operation with - * the server is done. The blocking time varies by communication latency. + * @warning This function blocks until the reply from the robot is received. */ void getPlanInfo(PlanInfo& output); /** - * @brief Execute a primitive by specifying its name and parameters, which - * can be found in the [Flexiv Primitives + * @brief [Blocking] Execute a primitive by specifying its name and + * parameters, which can be found in the [Flexiv Primitives * documentation](https://www.flexiv.com/primitives/). * @param[in] ptCmd Primitive command with the following string format: * "primitiveName(inputParam1=xxx, inputParam2=xxx, ...)". - * @note Applicable operation mode: NRT_PRIMITIVE_EXECUTION. * @throw InputException if size of the input string is greater than 5kb. - * @throw LogicException if robot is not in the correct operation mode. + * @throw LogicException if robot is not in the correct control mode. * @throw ExecutionException if error occurred during execution. - * @note A primitive won't terminate itself upon finish, thus isBusy() - * cannot be used to check if a primitive task is finished, use primitive - * states like "reachedTarget" instead. + * @note Applicable control mode: NRT_PRIMITIVE_EXECUTION. * @warning The primitive input parameters may not use SI units, please * refer to the Flexiv Primitives documentation for exact unit definition. * @warning Some primitives may not terminate automatically and require * users to manually terminate them based on specific primitive states, * for example, most [Move] primitives. In such case, isBusy() will stay * true even if it seems everything is done for that primitive. - * @warning This method will block for 50 ms for the non-real-time command - * to be transmitted and fully processed by the robot server. + * @warning This function blocks until the request is successfully delivered + * to the robot and fully processed. */ void executePrimitive(const std::string& ptCmd); /** - * @brief Get feedback states of the currently executing primitive. + * @brief [Blocking] Get feedback states of the currently executing + * primitive. * @return Primitive states in the format of a string list. - * @throw CommException if there's no response from server. + * @throw CommException if there's no response from the robot. * @throw ExecutionException if error occurred during execution. - * @warning This method will block until the request-reply operation with - * the server is done. The blocking time varies by communication latency. + * @warning This function blocks until the reply from the robot is received. */ std::vector getPrimitiveStates(void) const; /** - * @brief Set global variables for the robot by specifying name and value. + * @brief [Blocking] Set global variables for the robot by specifying name + * and value. * @param[in] globalVars Command to set global variables using the format: * globalVar1=value(s), globalVar2=value(s), ... - * @note Applicable operation mode: NRT_PLAN_EXECUTION. * @throw InputException if size of the input string is greater than 5kb. - * @throw LogicException if robot is not in the correct operation mode. + * @throw LogicException if robot is not in the correct control mode. * @throw ExecutionException if error occurred during execution. + * @note Applicable control mode: NRT_PLAN_EXECUTION. * @warning The specified global variable(s) must have already been created - * in the robot server, otherwise setting a nonexistent global variable will - * have no effect. To check if a global variable is successfully set, use - * getGlobalVariables(). - * @warning This method will block for 50 ms for the non-real-time command - * to be transmitted and fully processed by the robot server. + * in the robot using Flexiv Elements, otherwise setting a nonexistent + * global variable will have no effect. To check if a global variable is + * successfully set, use getGlobalVariables(). + * @warning This function blocks until the request is successfully delivered + * to the robot and fully processed. */ void setGlobalVariables(const std::string& globalVars); /** - * @brief Get available global variables from the robot. + * @brief [Blocking] Get available global variables from the robot. * @return Global variables in the format of a string list. - * @throw CommException if there's no response from server. + * @throw CommException if there's no response from the robot. * @throw ExecutionException if error occurred during execution. - * @warning This method will block until the request-reply operation with - * the server is done. The blocking time varies by communication latency. + * @warning This function blocks until the reply from the robot is received. */ std::vector getGlobalVariables(void) const; /** - * @brief Check if the robot has come to a complete stop. + * @brief [Non-blocking] Check if the robot has come to a complete stop. * @return True: stopped, false: still moving. */ bool isStopped(void) const; /** - * @brief If the mounted tool has more than one TCP, switch the TCP being - * used by the robot server. Default to the 1st one (index = 0). + * @brief [Blocking] If the mounted tool has more than one TCP, switch the + * TCP being used by the robot. Default to the 1st one (index = 0). * @param[in] index Index of the TCP on the mounted tool to switch to. - * @note No need to call this method if the mounted tool on the robot has - * only one TCP, it'll be used by default. * @throw ExecutionException if error occurred during execution. + * @note No need to call this function if the mounted tool on the robot has + * only one TCP, it'll be used by default. + * @note New TCP index will take effect upon control mode switch, or upon + * sending a new primitive command. + * @warning This function blocks until the request is successfully delivered + * to the robot. */ void switchTcp(unsigned int index); /** - * @brief Start auto recovery to bring joints that are outside the allowed - * position range back into allowed range. + * @brief [Blocking] Run automatic recovery to bring joints that are outside + * the allowed position range back into allowed range. + * @throw ExecutionException if error occurred during execution. * @note Refer to user manual for more details. + * @warning This function blocks until the automatic recovery process is + * finished. * @see isRecoveryState() - * @throw ExecutionException if error occurred during execution. */ - void startAutoRecovery(void); + void runAutoRecovery(void); - //====================== DIRECT MOTION/FORCE CONTROL ======================= + //========================== DIRECT JOINT CONTROL ========================== /** - * @brief Continuously send joint torque command to robot. + * @brief [Non-blocking] Continuously stream joint torque command to the + * robot. * @param[in] torques Target joint torques: \f$ {\tau_J}_d \in * \mathbb{R}^{DOF \times 1} \f$. Unit: \f$ [Nm] \f$. * @param[in] enableGravityComp Enable/disable robot gravity compensation. * @param[in] enableSoftLimits Enable/disable soft limits to keep the * joints from moving outside the allowed position range, which will * trigger a safety fault that requires recovery operation. - * @note Applicable operation mode: RT_JOINT_TORQUE. - * @note Real-time (RT). * @throw InputException if input is invalid. - * @throw LogicException if robot is not in the correct operation mode. + * @throw LogicException if robot is not in the correct control mode. * @throw ExecutionException if error occurred during execution. + * @note Applicable control mode: RT_JOINT_TORQUE. + * @note Real-time (RT). * @warning Always stream smooth and continuous commands to avoid sudden * movements. */ @@ -327,19 +353,19 @@ class Robot bool enableGravityComp = true, bool enableSoftLimits = true); /** - * @brief Continuously send joint position, velocity, and acceleration - * command. + * @brief [Non-blocking] Continuously stream joint position, velocity, and + * acceleration command to the robot. * @param[in] positions Target joint positions: \f$ q_d \in \mathbb{R}^{DOF * \times 1} \f$. Unit: \f$ [rad] \f$. * @param[in] velocities Target joint velocities: \f$ \dot{q}_d \in * \mathbb{R}^{DOF \times 1} \f$. Unit: \f$ [rad/s] \f$. * @param[in] accelerations Target joint accelerations: \f$ \ddot{q}_d \in * \mathbb{R}^{DOF \times 1} \f$. Unit: \f$ [rad/s^2] \f$. - * @note Applicable operation mode: RT_JOINT_POSITION. - * @note Real-time (RT). * @throw InputException if input is invalid. - * @throw LogicException if robot is not in the correct operation mode. + * @throw LogicException if robot is not in the correct control mode. * @throw ExecutionException if error occurred during execution. + * @note Applicable control mode: RT_JOINT_POSITION. + * @note Real-time (RT). * @warning Always stream smooth and continuous commands to avoid sudden * movements. */ @@ -348,191 +374,315 @@ class Robot const std::vector& accelerations); /** - * @brief Discretely send joint position, velocity, and acceleration - * command. The internal trajectory generator will interpolate between two - * set points and make the motion smooth. + * @brief [Non-blocking] Discretely send joint position, velocity, and + * acceleration command. The robot's internal motion generator will smoothen + * the discrete commands. * @param[in] positions Target joint positions: \f$ q_d \in \mathbb{R}^{DOF * \times 1} \f$. Unit: \f$ [rad] \f$. * @param[in] velocities Target joint velocities: \f$ \dot{q}_d \in - * \mathbb{R}^{DOF \times 1} \f$. Unit: \f$ [rad/s] \f$. + * \mathbb{R}^{DOF \times 1} \f$. Each joint will maintain this amount of + * velocity when it reaches the target position. Unit: \f$ [rad/s] \f$. * @param[in] accelerations Target joint accelerations: \f$ \ddot{q}_d \in - * \mathbb{R}^{DOF \times 1} \f$. Unit: \f$ [rad/s^2] \f$. - * @param[in] maxVel Maximum joint velocities: \f$ \dot{q}_{max} \in - * \mathbb{R}^{DOF \times 1} \f$. Unit: \f$ [rad/s] \f$. - * @param[in] maxAcc Maximum joint accelerations: \f$ \ddot{q}_{max} \in - * \mathbb{R}^{DOF \times 1} \f$. Unit: \f$ [rad/s^2] \f$. - * @note Applicable operation mode: NRT_JOINT_POSITION. + * \mathbb{R}^{DOF \times 1} \f$. Each joint will maintain this amount of + * acceleration when it reaches the target position. Unit: \f$ [rad/s^2] + * \f$. + * @param[in] maxVel Maximum joint velocities for the planned trajectory: + * \f$ \dot{q}_{max} \in \mathbb{R}^{DOF \times 1} \f$. Unit: \f$ [rad/s] + * \f$. + * @param[in] maxAcc Maximum joint accelerations for the planned trajectory: + * \f$ \ddot{q}_{max} \in \mathbb{R}^{DOF \times 1} \f$. Unit: \f$ [rad/s^2] + * \f$. * @throw InputException if input is invalid. - * @throw LogicException if robot is not in the correct operation mode. + * @throw LogicException if robot is not in the correct control mode. * @throw ExecutionException if error occurred during execution. + * @note Applicable control mode: NRT_JOINT_POSITION. + * @warning Calling this function a second time while the motion from the + * previous call is still ongoing will trigger an online re-planning of + * the joint trajectory, such that the previous command is aborted and the + * new command starts to execute. */ void sendJointPosition(const std::vector& positions, const std::vector& velocities, const std::vector& accelerations, const std::vector& maxVel, const std::vector& maxAcc); + //======================== DIRECT CARTESIAN CONTROL ======================== /** - * @brief Continuously command Cartesian motion and force command for the - * robot to track using its unified motion-force controller. - * @param[in] pose Target TCP pose in base or TCP frame (depends on - * operation mode): \f$ {^{O}T_{TCP}}_{d} \f$ or \f$ {^{TCP}T_{TCP}}_{d} \in + * @brief [Non-blocking] Continuously stream Cartesian motion and/or force + * command for the robot to track using its unified motion-force controller, + * which allows doing force control in zero or more Cartesian axes and + * motion control in the rest axes. + * @param[in] pose Target TCP pose in base frame: \f$ {^{O}T_{TCP}}_{d} \in * \mathbb{R}^{7 \times 1} \f$. Consists of \f$ \mathbb{R}^{3 \times 1} \f$ * position and \f$ \mathbb{R}^{4 \times 1} \f$ quaternion: \f$ [x, y, z, * q_w, q_x, q_y, q_z]^T \f$. Unit: \f$ [m]~[] \f$. - * @param[in] wrench Target TCP wrench in base or TCP frame (depends on - * operation mode): \f$ ^{0}F_d \f$ or \f$ ^{TCP}F_d \in \mathbb{R}^{6 - * \times 1} \f$. If TCP frame is used, unlike motion control, the reference - * frame for force control is always the robot's current TCP frame. When the - * target value of a direction is set to non-zero, this direction will - * smoothly transit from motion control to force control, and the robot will - * track the target force/moment in this direction using an explicit force - * controller. When the target value is reset to 0, this direction will then - * smoothly transit from force control back to motion control, and the robot - * will gently move to the target motion point even if the set point is - * distant. Calling with default parameter (all zeros) will result in pure - * motion control in all directions. Consists of \f$ \mathbb{R}^{3 \times 1} - * \f$ force and \f$ \mathbb{R}^{3 \times 1} \f$ moment: \f$ [f_x, f_y, f_z, - * m_x, m_y, m_z]^T \f$. Unit: \f$ [N]~[Nm] \f$. - * @note Applicable operation modes: RT_CARTESIAN_MOTION_FORCE_BASE, - * RT_CARTESIAN_MOTION_FORCE_TCP. - * @note Real-time (RT). + * @param[in] wrench Target TCP wrench (force and moment) in the force + * control reference frame (configured by setForceControlFrame()): \f$ + * ^{0}F_d \in \mathbb{R}^{6 \times 1} \f$. The robot will track the target + * wrench using an explicit force controller. Consists of \f$ \mathbb{R}^{3 + * \times 1} \f$ force and \f$ \mathbb{R}^{3 \times 1} \f$ moment: \f$ [f_x, + * f_y, f_z, m_x, m_y, m_z]^T \f$. Unit: \f$ [N]~[Nm] \f$. * @throw InputException if input is invalid. - * @throw LogicException if robot is not in the correct operation mode. + * @throw LogicException if robot is not in the correct control mode. * @throw ExecutionException if error occurred during execution. - * @warning Reference frame non-orthogonality between motion- and force- - * controlled directions can happen when using the TCP frame mode - * (RT_CARTESIAN_MOTION_FORCE_TCP). The reference frame for motion control - * is defined as the robot TCP frame at the time point when the operation - * mode is switched into RT_CARTESIAN_MOTION_FORCE_TCP and is updated only - * upon each mode entrance, since motion control requires a fixed reference - * frame. The reference frame for force control is defined as the current - * (latest) robot TCP frame, since force control does not require a fixed - * reference frame. Such difference in frame definition means that, when - * force control is enabled for one or more directions, the force-controlled - * directions and motion-controlled directions are not guaranteed to stay - * orthogonal to each other. When non-orthogonality happens, the affected - * directions will see some control performance degradation. To avoid - * reference frame non-orthogonality and retain maximum control performance, - * it's recommended to keep the robot's Cartesian orientation unchanged when - * running motion-force control in TCP frame mode. Note that the base frame - * mode (RT_CARTESIAN_MOTION_FORCE_BASE) does not have such restriction. - * @warning Always stream smooth and continuous commands to avoid sudden - * movements. + * @note Applicable control modes: RT_CARTESIAN_MOTION_FORCE. + * @note Real-time (RT). + * @warning Always stream smooth and continuous motion commands to avoid + * sudden movements. The force commands don't need to be continuous. + * @par How to achieve pure motion control? + * Use setForceControlAxis() to disable force control for all Cartesian axes + * to achieve pure motion control. This function does pure motion control + * out of the box. + * @par How to achieve pure force control? + * Use setForceControlAxis() to enable force control for all Cartesian axes + * to achieve pure force control, active or passive. + * @par How to achieve unified motion-force control? + * Use setForceControlAxis() to enable force control for one or more + * Cartesian axes and leave the rest axes motion-controlled, then provide + * target pose for the motion-controlled axes and target wrench for the + * force-controlled axes. + * @see setCartesianStiffness(), setMaxContactWrench(), + * setNullSpacePosture(), setForceControlAxis(), setForceControlFrame(), + * setPassiveForceControl(). */ void streamCartesianMotionForce(const std::vector& pose, - const std::vector& wrench = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}); + const std::vector& wrench = std::vector(6)); /** - * @brief Discretely command Cartesian motion and force command for the - * robot to track using its unified motion-force controller. An internal - * motion generator will smooth the discrete commands. - * @param[in] pose Target TCP pose in base or TCP frame (depends on - * operation mode): \f$ {^{O}T_{TCP}}_{d} \f$ or \f$ {^{TCP}T_{TCP}}_{d} \in + * @brief [Non-blocking] Discretely send Cartesian motion and/or force + * command for the robot to track using its unified motion-force controller, + * which allows doing force control in zero or more Cartesian axes and + * motion control in the rest axes. The robot's internal motion generator + * will smoothen the discrete commands. + * @param[in] pose Target TCP pose in base frame: \f$ {^{O}T_{TCP}}_{d} \in * \mathbb{R}^{7 \times 1} \f$. Consists of \f$ \mathbb{R}^{3 \times 1} \f$ * position and \f$ \mathbb{R}^{4 \times 1} \f$ quaternion: \f$ [x, y, z, * q_w, q_x, q_y, q_z]^T \f$. Unit: \f$ [m]~[] \f$. - * @param[in] wrench Target TCP wrench in base or TCP frame (depends on - * operation mode): \f$ ^{0}F_d \f$ or \f$ ^{TCP}F_d \in \mathbb{R}^{6 - * \times 1} \f$. If TCP frame is used, unlike motion control, the reference - * frame for force control is always the robot's current TCP frame. When the - * target value of a direction is set to non-zero, this direction will - * smoothly transit from motion control to force control, and the robot will - * track the target force/moment in this direction using an explicit force - * controller. When the target value is reset to 0, this direction will then - * smoothly transit from force control back to motion control, and the robot - * will gently move to the target motion point even if the set point is - * distant. Calling with default parameter (all zeros) will result in pure - * motion control in all directions. Consists of \f$ \mathbb{R}^{3 \times 1} - * \f$ force and \f$ \mathbb{R}^{3 \times 1} \f$ moment: \f$ [f_x, f_y, f_z, - * m_x, m_y, m_z]^T \f$. Unit: \f$ [N]~[Nm] \f$. - * @note Applicable operation modes: NRT_CARTESIAN_MOTION_FORCE_BASE, - * NRT_CARTESIAN_MOTION_FORCE_TCP. - * @note Real-time (RT). + * @param[in] wrench Target TCP wrench (force and moment) in the force + * control reference frame (configured by setForceControlFrame()): \f$ + * ^{0}F_d \in \mathbb{R}^{6 \times 1} \f$. The robot will track the target + * wrench using an explicit force controller. Consists of \f$ \mathbb{R}^{3 + * \times 1} \f$ force and \f$ \mathbb{R}^{3 \times 1} \f$ moment: \f$ [f_x, + * f_y, f_z, m_x, m_y, m_z]^T \f$. Unit: \f$ [N]~[Nm] \f$. + * @param[in] maxLinearVel Maximum Cartesian linear velocity when moving to + * the target pose. Default maximum linear velocity is used when set to 0. + * Unit: \f$ [m/s] \f$. + * @param[in] maxAngularVel Maximum Cartesian angular velocity when moving + * to the target pose. Default maximum angular velocity is used when set to + * 0. Unit: \f$ [rad/s] \f$. * @throw InputException if input is invalid. - * @throw LogicException if robot is not in the correct operation mode. + * @throw LogicException if robot is not in the correct control mode. * @throw ExecutionException if error occurred during execution. - * @warning Reference frame non-orthogonality between motion- and force- - * controlled directions can happen when using the TCP frame mode - * (NRT_CARTESIAN_MOTION_FORCE_TCP). The reference frame for motion control - * is defined as the robot TCP frame at the time point when the operation - * mode is switched into NRT_CARTESIAN_MOTION_FORCE_TCP and is updated only - * upon each mode entrance, since motion control requires a fixed reference - * frame. The reference frame for force control is defined as the current - * (latest) robot TCP frame, since force control does not require a fixed - * reference frame. Such difference in frame definition means that, when - * force control is enabled for one or more directions, the force-controlled - * directions and motion-controlled directions are not guaranteed to stay - * orthogonal to each other. When non-orthogonality happens, the affected - * directions will see some control performance degradation. To avoid - * reference frame non-orthogonality and retain maximum control performance, - * it's recommended to keep the robot's Cartesian orientation unchanged when - * running motion-force control in TCP frame mode. Note that the base frame - * mode (NRT_CARTESIAN_MOTION_FORCE_BASE) does not have such restriction. + * @note Applicable control modes: NRT_CARTESIAN_MOTION_FORCE. + * @par How to achieve pure motion control? + * Use setForceControlAxis() to disable force control for all Cartesian axes + * to achieve pure motion control. This function does pure motion control + * out of the box. + * @par How to achieve pure force control? + * Use setForceControlAxis() to enable force control for all Cartesian axes + * to achieve pure force control, active or passive. + * @par How to achieve unified motion-force control? + * Use setForceControlAxis() to enable force control for one or more + * Cartesian axes and leave the rest axes motion-controlled, then provide + * target pose for the motion-controlled axes and target wrench for the + * force-controlled axes. + * @see setCartesianStiffness(), setMaxContactWrench(), + * setNullSpacePosture(), setForceControlAxis(), setForceControlFrame(), + * setPassiveForceControl(). */ void sendCartesianMotionForce(const std::vector& pose, - const std::vector& wrench = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}); - - /** - * @brief Set motion stiffness for the Cartesian motion-force control modes. - * @param[in] stiffness Desired Cartesian motion stiffness: \f$ K_d \in - * \mathbb{R}^{6 \times 1} \f$. Calling with default parameter (all zeros) - * will reset to the robot's nominal stiffness. Consists of \f$ - * \mathbb{R}^{3 \times 1} \f$ linear stiffness and \f$ \mathbb{R}^{3 \times - * 1} \f$ angular stiffness: \f$ [k_x, k_y, k_z, k_{Rx}, k_{Ry}, - * k_{Rz}]^T \f$. Unit: \f$ [N/m]~[Nm/rad] \f$. - * @note Applicable operation modes: RT/NRT_CARTESIAN_MOTION_FORCE_BASE, - * RT/NRT_CARTESIAN_MOTION_FORCE_TCP. + const std::vector& wrench = std::vector(6), + double maxLinearVel = 0.0, double maxAngularVel = 0.0); + + /** + * @brief [Non-blocking] Set motion stiffness for the Cartesian motion-force + * control modes. + * @param[in] stiffness Cartesian motion stiffness: \f$ K_d \in + * \mathbb{R}^{6 \times 1} \f$. Setting motion stiffness of a + * motion-controlled Cartesian axis to 0 will make this axis free-floating. + * Consists of \f$ \mathbb{R}^{3 \times 1} \f$ linear stiffness and \f$ + * \mathbb{R}^{3 \times 1} \f$ angular stiffness: \f$ [k_x, k_y, k_z, + * k_{Rx}, k_{Ry}, k_{Rz}]^T \f$. Valid range: 0 to RobotInfo::nominalK. + * Unit: \f$ [N/m]~[Nm/rad] \f$. * @throw InputException if input is invalid. - * @throw LogicException if robot is not in the correct operation mode. + * @throw LogicException if robot is not in the correct control mode. * @throw ExecutionException if error occurred during execution. + * @note Applicable control modes: RT/NRT_CARTESIAN_MOTION_FORCE. + * @warning The robot will automatically reset to its nominal stiffness upon + * re-entering the applicable control modes. */ - void setCartesianStiffness( - const std::vector& stiffness = {0, 0, 0, 0, 0, 0}); + void setCartesianStiffness(const std::vector& stiffness); /** - * @brief Set preferred joint positions for the null-space posture control - * module used in the Cartesian motion-force control modes. + * @brief [Non-blocking] Reset motion stiffness for the Cartesian + * motion-force control modes to nominal value. + * @note Applicable control modes: RT/NRT_CARTESIAN_MOTION_FORCE. + */ + void resetCartesianStiffness(void); + + /** + * @brief [Non-blocking] Set maximum contact wrench for the motion control + * part of the Cartesian motion-force control modes. The controller will + * regulate its output to maintain contact wrench (force and moment) with + * the environment under the set values. + * @param[in] maxWrench Maximum contact wrench (force and moment): \f$ F_max + * \in \mathbb{R}^{6 \times 1} \f$. Consists of \f$ \mathbb{R}^{3 \times 1} + * \f$ maximum force and \f$ \mathbb{R}^{3 \times 1} \f$ maximum moment: \f$ + * [f_x, f_y, f_z, m_x, m_y, m_z]^T \f$. Unit: \f$ [N]~[Nm] \f$. + * @throw InputException if input is invalid. + * @throw LogicException if robot is not in the correct control mode. + * @note The maximum contact wrench regulation only applies to the motion + * control part. + * @note Applicable control modes: RT/NRT_CARTESIAN_MOTION_FORCE. + * @warning The maximum contact wrench regulation will automatically reset + * to disabled upon re-entering the applicable control modes. + * @warning The maximum contact wrench regulation cannot be enabled if any + * of the rotational Cartesian axes is enabled for moment control. + */ + void setMaxContactWrench(const std::vector& maxWrench); + + /** + * @brief [Non-blocking] Reset max contact wrench regulation to nominal + * state, i.e. disabled. + * @note Applicable control modes: RT/NRT_CARTESIAN_MOTION_FORCE. + */ + void resetMaxContactWrench(void); + + /** + * @brief [Non-blocking] Set preferred joint positions for the null-space + * posture control module used in the Cartesian motion-force control modes. * @param[in] preferredPositions Preferred joint positions for the * null-space posture control: \f$ q_{ns} \in \mathbb{R}^{DOF \times 1} \f$. - * Calling with default parameter (all zeros) will reset to the robot's - * nominal preferred joint positions, which is the home posture. * Unit: \f$ [rad] \f$. + * @throw InputException if input is invalid. + * @throw LogicException if robot is not in the correct control mode. + * @throw ExecutionException if error occurred during execution. + * @note Applicable control modes: RT/NRT_CARTESIAN_MOTION_FORCE. + * @warning The robot will automatically reset to its nominal preferred + * joint positions upon re-entering the applicable control modes. * @par Null-space posture control * Similar to human arm, a robotic arm with redundant degree(s) of * freedom (DOF > 6) can change its overall posture without affecting the * ongoing primary task. This is achieved through a technique called * "null-space control". After the preferred joint positions for a desired - * robot posture is set using this method, the robot's null-space control + * robot posture is set using this function, the robot's null-space control * module will try to pull the arm as close to this posture as possible * without affecting the primary Cartesian motion-force control task. - * @note Applicable operation modes: RT/NRT_CARTESIAN_MOTION_FORCE_BASE, - * RT/NRT_CARTESIAN_MOTION_FORCE_TCP. - * @throw InputException if input is invalid. - * @throw LogicException if robot is not in the correct operation mode. - * @throw ExecutionException if error occurred during execution. */ - void setNullSpacePosture( - const std::vector& preferredPositions = {0, 0, 0, 0, 0, 0, 0}); + void setNullSpacePosture(const std::vector& preferredPositions); - //=============================== IO CONTROL =============================== /** - * @brief Set digital output on the control box. - * @param[in] portNumber Port to set value to [0 ~ 15]. - * @param[in] value True: set high, false: set low. - * @throw ExecutionException if error occurred during execution. - * @throw InputException if input is invalid. + * @brief [Non-blocking] Reset preferred joint positions to the robot's home + * posture. + * @note Applicable control modes: RT/NRT_CARTESIAN_MOTION_FORCE. */ - void writeDigitalOutput(unsigned int portNumber, bool value); + void resetNullSpacePosture(void); /** - * @brief Read digital input on the control box. - * @param[in] portNumber Port to read value from [0 ~ 15]. - * @return True: port high, false: port low. - * @throw CommException if there's no response from server. - * @throw ExecutionException if error occurred during execution. + * @brief [Blocking] Set force-controlled Cartesian axis(s) for the + * Cartesian motion-force control modes. The axis(s) not enabled for force + * control will be motion controlled. This function can only be called when + * the robot is in IDLE mode. + * @param[in] enabledAxis Flags to enable/disable force control for certain + * Cartesian axis(s) in the force control reference frame (configured by + * setForceControlFrame()). The corresponding order is \f$ [X, Y, Z, Rx, Ry, + * Rz] \f$. By default, force control is disabled for all Cartesian axes. * @throw InputException if input is invalid. - */ - bool readDigitalInput(unsigned int portNumber); + * @throw LogicException if robot is not in the correct control mode. + * @throw ExecutionException if failed to execute the request. + * @note Applicable control modes: IDLE. + * @warning This setting will reset to all axes disabled upon disconnection. + * @warning This function blocks until the request is successfully delivered + * to the robot. + */ + void setForceControlAxis(const std::vector& enabledAxis); + + /** + * @brief [Blocking] Set force control reference frame for the Cartesian + * motion-force control modes. This function can only be called when the + * robot is in IDLE mode. + * @param[in] referenceFrame The reference frame to use for force control. + * Options are: "TCP" and "BASE". The target wrench and force control axis + * should also be expressed in the selected reference frame. By default, + * base frame is used for force control. + * @throw InputException if input is invalid. + * @throw LogicException if robot is not in the correct control mode. + * @throw ExecutionException if failed to execute the request. + * @note Applicable control modes: IDLE. + * @warning This setting will reset to robot base frame upon disconnection. + * @warning This function blocks until the request is successfully delivered + * to the robot. + * @par Force control reference frame + * In Cartesian motion-force control modes, the reference frame of motion + * control is always the robot base frame, but the reference frame of force + * control can be either robot base frame or the robot's current TCP frame. + * While the robot base frame is the commonly used global coordinate, the + * current TCP frame is a dynamic local coordinate whose transformation with + * regard to the robot base frame changes as the robot TCP moves. When using + * robot base frame for force control, the force-controlled axis(s) and + * motion-controlled axis(s) are guaranteed to be orthogonal. However, when + * using current TCP frame for force control, the force-controlled axis(s) + * and motion-controlled axis(s) are NOT guaranteed to be orthogonal because + * different reference frames are used. In this case, it's recommended but + * not required to set the target pose such that the actual robot motion + * direction(s) are orthogonal to force direction(s). If they are not + * orthogonal, the motion control's vector component(s) in the force + * direction(s) will be eliminated. + */ + void setForceControlFrame(const std::string& referenceFrame); + + /** + * @brief [Blocking] Enable or disable passive force control for the + * Cartesian motion-force control modes. When enabled, an open-loop force + * controller will be used to feed forward the target wrench, i.e. passive + * force control. When disabled, a closed-loop force controller will be used + * to track the target wrench, i.e. active force control. This function can + * only be called when the robot is in IDLE mode. + * @param[in] isEnabled True: enable, false: disable. By default, passive + * force control is disabled and active force control is used. + * @throw LogicException if robot is not in the correct control mode. + * @throw ExecutionException if failed to execute the request. + * @note Applicable control modes: IDLE. + * @warning This setting will reset to disabled upon disconnection. + * @warning This function blocks until the request is successfully delivered + * to the robot. + * @par Difference between active and passive force control + * Active force control uses a feedback loop to reduce the error between + * target wrench and measured wrench. This method results in better force + * tracking performance, but at the cost of additional Cartesian damping + * which could potentially decrease motion tracking performance. On the + * other hand, passive force control simply feeds forward the target wrench. + * This methods results in worse force tracking performance, but is more + * robust and does not introduce additional Cartesian damping. The choice of + * active or passive force control depends on the actual application. + */ + void setPassiveForceControl(bool isEnabled); + + //=============================== IO CONTROL =============================== + /** + * @brief [Blocking] Write to single or multiple digital output port(s) on + * the control box. + * @param[in] portIdx Index of port(s) to write, can be a single port or + * multiple ports. E.g. {0, 5, 7, 15} or {1, 3, 10} or {8}. Valid range of + * the index number is [0–15]. + * @param[in] values Corresponding values to write to the specified ports. + * True: set port high, false: set port low. Vector size must match the size + * of portIdx. + * @throw InputException if any index number in portIdx is not within + * [0–15], or if the two input vectors have different sizes. + * @throw ExecutionException if failed to execute the request. + * @warning This function blocks until the request is successfully delivered + * to the robot. + */ + void writeDigitalOutput(const std::vector& portIdx, + const std::vector& values); + + /** + * @brief [Non-blocking] Read all digital input ports on the control box. + * @return Digital input readings array whose index corresponds to the + * digital input port index. True: port high, false: port low. + */ + std::array readDigitalInput(void); private: class Impl; diff --git a/include/flexiv/Scheduler.hpp b/include/flexiv/Scheduler.hpp index 02cde652..10cbe9ac 100644 --- a/include/flexiv/Scheduler.hpp +++ b/include/flexiv/Scheduler.hpp @@ -21,33 +21,37 @@ class Scheduler { public: /** - * @brief Create and initialize a flexiv::Scheduler instance. - * @throw InitException if the instance failed to initialize. + * @brief [Blocking] Create a flexiv::Scheduler instance and initialize the + * real-time scheduler. + * @throw InitException if the initialization sequence failed. * @throw ClientException if an error is triggered by the client computer. + * @warning This constructor blocks until the initialization sequence is + * successfully finished. */ Scheduler(); virtual ~Scheduler(); /** - * @brief Add a new periodic task to the scheduler's task pool. Each task in - * the pool is assigned to a dedicated thread with independent thread - * configuration. + * @brief [Non-blocking] Add a new periodic task to the scheduler's task + * pool. Each task in the pool is assigned to a dedicated thread with + * independent thread configuration. * @param[in] callback Callback function of user task. * @param[in] taskName A unique name for this task. * @param[in] interval Execution interval of this periodic task [ms]. The * minimum available interval is 1 ms, equivalent to 1 kHz loop frequency. - * @param[in] priority Priority for this task thread, can be set to 0 ~ - * , with 0 being the lowest, and being the - * highest. can be obtained from maxPriority(). When the - * priority is set to non-zero, this thread becomes a real-time thread and - * can only be interrupted by threads with higher priority. When the - * priority is set to 0, this thread becomes a non-real-time thread and can - * be interrupted by any real-time threads. The common practice is to set - * priority of the most critical tasks to or near, and set - * priority of other non-critical tasks to 0 or near. To avoid race - * conditions, the same priority should be assigned to only one task. + * @param[in] priority Priority for this task thread, can be set to + * minPriority()–maxPriority() for real-time scheduling, or 0 for + * non-real-time scheduling. When the priority is set to use real-time + * scheduling, this thread becomes a real-time thread and can only be + * interrupted by threads with higher priority. When the priority is set to + * use non-real-time scheduling (i.e. 0), this thread becomes a + * non-real-time thread and can be interrupted by any real-time threads. The + * common practice is to set priority of the most critical tasks to + * maxPriority() or near, and set priority of other non-critical tasks to 0 + * or near. To avoid race conditions, the same priority should be assigned + * to only one task. * @param[in] cpuAffinity CPU core for this task thread to bind to, can be - * set to 2 ~ ( - 1). This task thread will only run on + * set to 2–(num_cores - 1). This task thread will only run on * the specified CPU core. If left with the default value (-1), then this * task thread will not bind to any CPU core, and the system will decide * which core to run this task thread on according to the system's own @@ -71,34 +75,43 @@ class Scheduler int cpuAffinity = -1); /** - * @brief Start to execute all added tasks periodically. - * @param[in] isBlocking Whether to block the thread from which this - * method is called until the scheduler is stopped. A common usage is to - * call this method from main() with this parameter set to true to keep - * main() from returning. - * @throw LogicException if the scheduler is not fully initialized yet. - * @throw ClientException if an error is triggered by the client computer. + * @brief [Blocking] Start all added tasks. A dedicated thread will be + * created for each added task and the periodic execution will begin. + * @throw LogicException if the scheduler is not initialized yet. + * @throw ClientException if failed to start the tasks. + * @warning This function blocks until all added tasks are started. */ - void start(bool isBlocking = true); + void start(); /** - * @brief Stop all added tasks. start() will stop blocking and return. - * @throw LogicException if the scheduler is not started or is not fully - * initialized yet. - * @throw ClientException if an error is triggered by the client computer. - * @note Call start() again to restart the scheduler. + * @brief [Blocking] Stop all added tasks. The periodic execution will stop + * and all task threads will be closed with the resources released. + * @throw LogicException if the scheduler is not initialized or the tasks + * are not started yet. + * @throw ClientException if failed to stop the tasks. + * @note Calling start() again can restart the added tasks. + * @warning This function blocks until all task threads have exited and + * resources are released. + * @warning This function cannot be called from within a task thread. */ void stop(); /** - * @brief Get maximum available priority for the user task. - * @return The maximum priority that can be set to a user task when calling - * addTask(). + * @brief [Non-blocking] Get maximum available priority for user tasks. + * @return The maximum priority that can be set for a user task with + * real-time scheduling policy when calling addTask(). */ int maxPriority() const; /** - * @brief Get number of tasks added to the scheduler. + * @brief [Non-blocking] Get minimum available priority for user tasks. + * @return The minimum priority that can be set for a user task with + * real-time scheduling policy when calling addTask(). + */ + int minPriority() const; + + /** + * @brief [Non-blocking] Get number of tasks added to the scheduler. * @return Number of added tasks. */ size_t numTasks() const; diff --git a/include/flexiv/Utility.hpp b/include/flexiv/Utility.hpp index 90a38a21..b3fefc9d 100644 --- a/include/flexiv/Utility.hpp +++ b/include/flexiv/Utility.hpp @@ -8,6 +8,7 @@ #include #include +#include #include namespace flexiv { @@ -88,6 +89,34 @@ inline std::string vec2Str( return ss.str(); } +/** + * @brief Convert a std::array to a string. + * @param[in] arr std::array of any type and size. + * @param[in] decimal Decimal places to keep for each number in the array. + * @param[in] trailingSpace Whether to include a space after the last element. + * @return A string with format "arr[0] arr[1] ... arr[n] ", i.e. each element + * followed by a space, including the last one if trailingSpace = true. + */ +template +inline std::string arr2Str( + const std::array& arr, size_t decimal = 3, bool trailingSpace = true) +{ + auto padding = ""; + std::stringstream ss; + ss.precision(decimal); + ss << std::fixed; + + for (const auto& v : arr) { + ss << padding << v; + padding = " "; + } + + if (trailingSpace) { + ss << " "; + } + return ss.str(); +} + /** * @brief Check if any provided strings exist in the program arguments. * @param[in] argc Argument count passed to main() of the program. diff --git a/lib/FlexivRdk.win_amd64.lib b/lib/FlexivRdk.win_amd64.lib deleted file mode 100644 index 553b8806..00000000 Binary files a/lib/FlexivRdk.win_amd64.lib and /dev/null differ diff --git a/lib/flexiv_rdk.win_amd64.lib b/lib/flexiv_rdk.win_amd64.lib new file mode 100644 index 00000000..6e8d505b Binary files /dev/null and b/lib/flexiv_rdk.win_amd64.lib differ diff --git a/lib/libFlexivRdk.aarch64-linux-gnu.a b/lib/libflexiv_rdk.aarch64-linux-gnu.a similarity index 60% rename from lib/libFlexivRdk.aarch64-linux-gnu.a rename to lib/libflexiv_rdk.aarch64-linux-gnu.a index b20c970e..88d0cdaf 100644 Binary files a/lib/libFlexivRdk.aarch64-linux-gnu.a and b/lib/libflexiv_rdk.aarch64-linux-gnu.a differ diff --git a/lib/libFlexivRdk.arm64-darwin.a b/lib/libflexiv_rdk.arm64-darwin.a similarity index 56% rename from lib/libFlexivRdk.arm64-darwin.a rename to lib/libflexiv_rdk.arm64-darwin.a index eeda8af7..6739b43e 100644 Binary files a/lib/libFlexivRdk.arm64-darwin.a and b/lib/libflexiv_rdk.arm64-darwin.a differ diff --git a/lib/libFlexivRdk.x86_64-linux-gnu.a b/lib/libflexiv_rdk.x86_64-linux-gnu.a similarity index 61% rename from lib/libFlexivRdk.x86_64-linux-gnu.a rename to lib/libflexiv_rdk.x86_64-linux-gnu.a index 6538074b..3de295fd 100644 Binary files a/lib/libFlexivRdk.x86_64-linux-gnu.a and b/lib/libflexiv_rdk.x86_64-linux-gnu.a differ diff --git a/lib_py/flexivrdk.cp310-win_amd64.pyd b/lib_py/flexivrdk.cp310-win_amd64.pyd index 33e9f381..8725814f 100644 Binary files a/lib_py/flexivrdk.cp310-win_amd64.pyd and b/lib_py/flexivrdk.cp310-win_amd64.pyd differ diff --git a/lib_py/flexivrdk.cp38-win_amd64.pyd b/lib_py/flexivrdk.cp38-win_amd64.pyd index 3828ec0f..a46bf4d4 100644 Binary files a/lib_py/flexivrdk.cp38-win_amd64.pyd and b/lib_py/flexivrdk.cp38-win_amd64.pyd differ diff --git a/lib_py/flexivrdk.cpython-310-aarch64-linux-gnu.so b/lib_py/flexivrdk.cpython-310-aarch64-linux-gnu.so index b0e73eb8..3cf14c1f 100755 Binary files a/lib_py/flexivrdk.cpython-310-aarch64-linux-gnu.so and b/lib_py/flexivrdk.cpython-310-aarch64-linux-gnu.so differ diff --git a/lib_py/flexivrdk.cpython-310-darwin.so b/lib_py/flexivrdk.cpython-310-darwin.so index a811a3d3..4680434b 100755 Binary files a/lib_py/flexivrdk.cpython-310-darwin.so and b/lib_py/flexivrdk.cpython-310-darwin.so differ diff --git a/lib_py/flexivrdk.cpython-310-x86_64-linux-gnu.so b/lib_py/flexivrdk.cpython-310-x86_64-linux-gnu.so index 1a5cb293..a20f92ca 100755 Binary files a/lib_py/flexivrdk.cpython-310-x86_64-linux-gnu.so and b/lib_py/flexivrdk.cpython-310-x86_64-linux-gnu.so differ diff --git a/lib_py/flexivrdk.cpython-38-aarch64-linux-gnu.so b/lib_py/flexivrdk.cpython-38-aarch64-linux-gnu.so index 989433ab..283a819d 100755 Binary files a/lib_py/flexivrdk.cpython-38-aarch64-linux-gnu.so and b/lib_py/flexivrdk.cpython-38-aarch64-linux-gnu.so differ diff --git a/lib_py/flexivrdk.cpython-38-darwin.so b/lib_py/flexivrdk.cpython-38-darwin.so index 63eecf6b..03bce371 100755 Binary files a/lib_py/flexivrdk.cpython-38-darwin.so and b/lib_py/flexivrdk.cpython-38-darwin.so differ diff --git a/lib_py/flexivrdk.cpython-38-x86_64-linux-gnu.so b/lib_py/flexivrdk.cpython-38-x86_64-linux-gnu.so index b156fdb2..6bb7b3d4 100755 Binary files a/lib_py/flexivrdk.cpython-38-x86_64-linux-gnu.so and b/lib_py/flexivrdk.cpython-38-x86_64-linux-gnu.so differ diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index ea70b989..d9b55e47 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -1,8 +1,8 @@ - -cmake_minimum_required(VERSION 3.4) +cmake_minimum_required(VERSION 3.16.3) project(flexiv_rdk-tests) -set(CMAKE_CXX_STANDARD 14) +# Show verbose build info +SET(CMAKE_VERBOSE_MAKEFILE ON) message("OS: ${CMAKE_SYSTEM_NAME}") message("Processor: ${CMAKE_SYSTEM_PROCESSOR}") @@ -32,8 +32,12 @@ endif() # Find flexiv_rdk INTERFACE library find_package(flexiv_rdk REQUIRED) -# Build all selected examples +# Build all selected tests foreach(test ${TEST_LIST}) add_executable(${test} ${test}.cpp) target_link_libraries(${test} flexiv::flexiv_rdk) + # C++14 required + set_target_properties(${test} PROPERTIES + CXX_STANDARD 14 + CXX_STANDARD_REQUIRED ON) endforeach() diff --git a/test/test_dynamics_engine.cpp b/test/test_dynamics_engine.cpp index 62701e1d..ca6028a9 100644 --- a/test/test_dynamics_engine.cpp +++ b/test/test_dynamics_engine.cpp @@ -19,6 +19,7 @@ #include #include #include +#include namespace { /** J, M, G ground truth from MATLAB */ @@ -41,18 +42,19 @@ struct SharedData /** Mutex on shared data */ std::mutex g_mutex; +/** Atomic signal to stop scheduler tasks */ +std::atomic g_schedStop = {false}; } /** User-defined high-priority periodic task @ 1kHz */ -void highPriorityTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv::Log& log, - flexiv::Model& model, flexiv::RobotStates& robotStates) +void highPriorityTask( + flexiv::Robot& robot, flexiv::Log& log, flexiv::Model& model, flexiv::RobotStates& robotStates) { try { // Monitor fault on robot server if (robot.isFault()) { throw flexiv::ServerException( - "highPriorityTask: Fault occurred on robot server, exiting " - "..."); + "highPriorityTask: Fault occurred on robot server, exiting ..."); } // Mark timer start point @@ -62,7 +64,7 @@ void highPriorityTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv robot.getRobotStates(robotStates); // Update robot model in dynamics engine - model.updateModel(robotStates.q, robotStates.dtheta); + model.update(robotStates.q, robotStates.dtheta); // Get J, M, G from dynamic engine Eigen::MatrixXd J = model.getJacobian("flange"); @@ -84,7 +86,7 @@ void highPriorityTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv } catch (const flexiv::Exception& e) { log.error(e.what()); - scheduler.stop(); + g_schedStop = true; } } @@ -114,20 +116,17 @@ void lowPriorityTask() auto deltaG = G - g_groundTruth.G; std::cout << std::fixed << std::setprecision(5); - std::cout << "Difference of J between ground truth (MATLAB) and " - "integrated dynamics engine = " + 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() << '\n' << std::endl; - std::cout << "Difference of M between ground truth (MATLAB) and " - "integrated dynamics engine = " + 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() << '\n' << std::endl; - std::cout << "Difference of G between ground truth (MATLAB) and " - "integrated dynamics engine = " + 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() << '\n' << std::endl; @@ -190,15 +189,8 @@ int main(int argc, char* argv[]) robot.enable(); // Wait for the robot to become operational - int secondsWaited = 0; while (!robot.isOperational()) { std::this_thread::sleep_for(std::chrono::seconds(1)); - if (++secondsWaited == 10) { - log.warn( - "Still waiting for robot to become operational, please " - "check that the robot 1) has no fault, 2) is booted " - "into Auto mode"); - } } log.info("Robot is now operational"); @@ -250,14 +242,24 @@ int main(int argc, char* argv[]) //============================================================================= flexiv::Scheduler scheduler; // Add periodic task with 1ms interval and highest applicable priority - scheduler.addTask(std::bind(highPriorityTask, std::ref(robot), std::ref(scheduler), - std::ref(log), std::ref(model), std::ref(robotStates)), + scheduler.addTask(std::bind(highPriorityTask, std::ref(robot), std::ref(log), + std::ref(model), std::ref(robotStates)), "HP periodic", 1, scheduler.maxPriority()); // Add periodic task with 1s interval and lowest applicable priority scheduler.addTask(lowPriorityTask, "LP periodic", 1000, 0); - // Start all added tasks, this is by default a blocking method + // Start all added tasks scheduler.start(); + // Block and wait for signal to stop scheduler tasks + while (!g_schedStop) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + // Received signal to stop scheduler tasks + scheduler.stop(); + + // Wait a bit for any last-second robot log message to arrive and get printed + std::this_thread::sleep_for(std::chrono::seconds(2)); + } catch (const flexiv::Exception& e) { log.error(e.what()); return 1; diff --git a/test/test_dynamics_with_tool.cpp b/test/test_dynamics_with_tool.cpp index 56cb2c76..0f7c5a9f 100644 --- a/test/test_dynamics_with_tool.cpp +++ b/test/test_dynamics_with_tool.cpp @@ -17,6 +17,7 @@ #include #include #include +#include namespace { /** M, G ground truth from MATLAB */ @@ -37,25 +38,26 @@ struct SharedData /** Mutex on shared data */ std::mutex g_mutex; +/** Atomic signal to stop scheduler tasks */ +std::atomic g_schedStop = {false}; } /** User-defined high-priority periodic task @ 1kHz */ -void highPriorityTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv::Log& log, - flexiv::Model& model, flexiv::RobotStates& robotStates) +void highPriorityTask( + flexiv::Robot& robot, flexiv::Log& log, flexiv::Model& model, flexiv::RobotStates& robotStates) { try { // Monitor fault on robot server if (robot.isFault()) { throw flexiv::ServerException( - "highPriorityTask: Fault occurred on robot server, exiting " - "..."); + "highPriorityTask: Fault occurred on robot server, exiting ..."); } // Read robot states robot.getRobotStates(robotStates); // Update robot model in dynamics engine - model.updateModel(robotStates.q, robotStates.dtheta); + model.update(robotStates.q, robotStates.dtheta); // Mark timer start point auto tic = std::chrono::high_resolution_clock::now(); @@ -78,7 +80,7 @@ void highPriorityTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv } catch (const flexiv::Exception& e) { log.error(e.what()); - scheduler.stop(); + g_schedStop = true; } } @@ -178,15 +180,8 @@ int main(int argc, char* argv[]) robot.enable(); // Wait for the robot to become operational - int secondsWaited = 0; while (!robot.isOperational()) { std::this_thread::sleep_for(std::chrono::seconds(1)); - if (++secondsWaited == 10) { - log.warn( - "Still waiting for robot to become operational, please " - "check that the robot 1) has no fault, 2) is booted " - "into Auto mode"); - } } log.info("Robot is now operational"); @@ -245,14 +240,24 @@ int main(int argc, char* argv[]) //============================================================================= flexiv::Scheduler scheduler; // Add periodic task with 1ms interval and highest applicable priority - scheduler.addTask(std::bind(highPriorityTask, std::ref(robot), std::ref(scheduler), - std::ref(log), std::ref(model), std::ref(robotStates)), + scheduler.addTask(std::bind(highPriorityTask, std::ref(robot), std::ref(log), + std::ref(model), std::ref(robotStates)), "HP periodic", 1, scheduler.maxPriority()); // Add periodic task with 1s interval and lowest applicable priority scheduler.addTask(lowPriorityTask, "LP periodic", 1000, 0); - // Start all added tasks, this is by default a blocking method + // Start all added tasks scheduler.start(); + // Block and wait for signal to stop scheduler tasks + while (!g_schedStop) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + // Received signal to stop scheduler tasks + scheduler.stop(); + + // Wait a bit for any last-second robot log message to arrive and get printed + std::this_thread::sleep_for(std::chrono::seconds(2)); + } catch (const flexiv::Exception& e) { log.error(e.what()); return 1; diff --git a/test/test_endurance.cpp b/test/test_endurance.cpp index d37a5ffc..a903a84d 100644 --- a/test/test_endurance.cpp +++ b/test/test_endurance.cpp @@ -18,102 +18,81 @@ #include #include #include +#include namespace { - -// size of Cartesian pose vector [position 3x1 + rotation (quaternion) 4x1 ] -const unsigned int k_cartPoseSize = 7; - -// RT loop period [sec] +/** RT loop period [sec] */ const double k_loopPeriod = 0.001; -// TCP sine-sweep amplitude [m] +/** TCP sine-sweep amplitude [m] */ const double k_swingAmp = 0.1; -// TCP sine-sweep frequency [Hz] +/** TCP sine-sweep frequency [Hz] */ const double k_swingFreq = 0.025; // = 10mm/s linear velocity -// current Cartesian-space pose (position + rotation) of robot TCP +/** Current Cartesian-space pose (position + rotation) of robot TCP */ std::vector g_currentTcpPose; -// high-priority task loop counter -uint64_t g_hpLoopCounter = 0; - -// test duration converted from user-specified hours to loop counts +/** Test duration converted from user-specified hours to loop counts */ uint64_t g_testDurationLoopCounts = 0; -// time duration for each log file [loop counts] +/** Time duration for each log file [loop counts] */ const unsigned int k_logDurationLoopCounts = 10 * 60 * 1000; // = 10 min/file -// data to be logged in low-priority thread +/** Data to be logged in low-priority thread */ struct LogData { std::vector tcpPose; std::vector tcpForce; } g_logData; +/** Atomic signal to stop the test */ +std::atomic g_stop = {false}; } -void highPriorityTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv::Log& log, - flexiv::RobotStates& robotStates) +void highPriorityTask(flexiv::Robot& robot, flexiv::Log& log, flexiv::RobotStates& robotStates, + const std::vector& initPose) { - // flag whether initial Cartesian position is set - static bool isInitPoseSet = false; - - // Initial Cartesian-space pose (position + rotation) of robot TCP - static std::vector initTcpPose; + // Local periodic loop counter + static uint64_t loopCounter = 0; try { // Monitor fault on robot server if (robot.isFault()) { throw flexiv::ServerException( - "highPriorityTask: Fault occurred on robot server, exiting " - "..."); + "highPriorityTask: Fault occurred on robot server, exiting ..."); } // Read robot states robot.getRobotStates(robotStates); - // TCP movement control - //===================================================================== - // set initial TCP pose - if (!isInitPoseSet) { - // check vector size before saving - if (robotStates.tcpPose.size() == k_cartPoseSize) { - initTcpPose = robotStates.tcpPose; - g_currentTcpPose = initTcpPose; - isInitPoseSet = true; - } - } - // run control after initial pose is set - else { - // move along Z direction - g_currentTcpPose[2] - = initTcpPose[2] - + k_swingAmp * sin(2 * M_PI * k_swingFreq * g_hpLoopCounter * k_loopPeriod); - robot.streamCartesianMotionForce(g_currentTcpPose); - } + // Swing along Z direction + g_currentTcpPose[2] + = initPose[2] + k_swingAmp * sin(2 * M_PI * k_swingFreq * loopCounter * k_loopPeriod); + robot.streamCartesianMotionForce(g_currentTcpPose); - // save data to global buffer, not using mutex to avoid interruption on + // Save data to global buffer, not using mutex to avoid interruption on // RT loop from potential priority inversion g_logData.tcpPose = robotStates.tcpPose; g_logData.tcpForce = robotStates.extWrenchInBase; - // increment loop counter - g_hpLoopCounter++; + // Stop after test duration has elapsed + if (++loopCounter > g_testDurationLoopCounts) { + g_stop = true; + } } catch (const flexiv::Exception& e) { log.error(e.what()); - scheduler.stop(); + g_stop = true; } } void lowPriorityTask() { - // low-priority task loop counter - uint64_t lpLoopCounter = 0; + // Low-priority task loop counter + uint64_t loopCounter = 0; - // data logging CSV file + // Data logging CSV file std::ofstream csvFile; // CSV file name @@ -122,34 +101,32 @@ void lowPriorityTask() // CSV file counter (data during the test is divided to multiple files) unsigned int fileCounter = 0; - // log object for printing message with timestamp and coloring + // Log object for printing message with timestamp and coloring flexiv::Log log; - // wait for a while for the robot states data to be available + // Wait for a while for the robot states data to be available std::this_thread::sleep_for(std::chrono::seconds(2)); - // use while loop to prevent this thread from return + // Use while loop to prevent this thread from return while (true) { - // run at 1kHz + // Log data at 1kHz std::this_thread::sleep_for(std::chrono::milliseconds(1)); + loopCounter++; - // Data logging - //===================================================================== - // close existing log file and create a new one periodically - if (lpLoopCounter % k_logDurationLoopCounts == 0) { - // close file if exist + // Close existing log file and create a new one periodically + if (loopCounter % k_logDurationLoopCounts == 0) { if (csvFile.is_open()) { csvFile.close(); log.info("Saved log file: " + csvFileName); } - // increment log file counter + // Increment log file counter fileCounter++; - // create new file name using the updated counter as suffix + // Create new file name using the updated counter as suffix csvFileName = "endurance_test_data_" + std::to_string(fileCounter) + ".csv"; - // open new log file + // Open new log file csvFile.open(csvFileName); if (csvFile.is_open()) { log.info("Created new log file: " + csvFileName); @@ -158,40 +135,31 @@ void lowPriorityTask() } } - // log data to file in CSV format, avoid logging too much data otherwise - // the RT loop will not hold + // Log data to file in CSV format if (csvFile.is_open()) { - // loop counter x1, TCP pose x7, TCP external force x6 - csvFile << lpLoopCounter << ","; - + // Loop counter x1, TCP pose x7, TCP external force x6 + csvFile << loopCounter << ","; for (const auto& i : g_logData.tcpPose) { csvFile << i << ","; } - for (const auto& i : g_logData.tcpForce) { csvFile << i << ","; } - - // end of line + // End of line csvFile << '\n'; } - // check if the test duration has elapsed - if (g_hpLoopCounter > g_testDurationLoopCounts) { + // Check if the test duration has elapsed + if (g_stop) { log.info("Test duration has elapsed, saving any open log file ..."); - - // close log file + // Close log file if (csvFile.is_open()) { csvFile.close(); log.info("Saved log file: " + csvFileName); } - - // exit program + // Exit thread return; } - - // increment loop counter - lpLoopCounter++; } } @@ -260,23 +228,14 @@ int main(int argc, char* argv[]) robot.enable(); // Wait for the robot to become operational - int secondsWaited = 0; while (!robot.isOperational()) { std::this_thread::sleep_for(std::chrono::seconds(1)); - if (++secondsWaited == 10) { - log.warn( - "Still waiting for robot to become operational, please " - "check that the robot 1) has no fault, 2) is booted " - "into Auto mode"); - } } log.info("Robot is now operational"); // Bring Robot To Home //============================================================================= - // set mode after robot is operational robot.setMode(flexiv::Mode::NRT_PLAN_EXECUTION); - robot.executePlan("PLAN-Home"); // Wait fot the plan to finish @@ -284,18 +243,25 @@ int main(int argc, char* argv[]) std::this_thread::sleep_for(std::chrono::seconds(1)); } while (robot.isBusy()); - // set mode after robot is at home - robot.setMode(flexiv::Mode::RT_CARTESIAN_MOTION_FORCE_BASE); + // Switch mode after robot is at home + robot.setMode(flexiv::Mode::RT_CARTESIAN_MOTION_FORCE); + + // Set initial pose to current TCP pose + robot.getRobotStates(robotStates); + auto initPose = robotStates.tcpPose; + log.info("Initial TCP pose set to [position 3x1, rotation (quaternion) 4x1]: " + + flexiv::utility::vec2Str(initPose)); + g_currentTcpPose = initPose; // Periodic Tasks //============================================================================= flexiv::Scheduler scheduler; // Add periodic task with 1ms interval and highest applicable priority - scheduler.addTask(std::bind(highPriorityTask, std::ref(robot), std::ref(scheduler), - std::ref(log), std::ref(robotStates)), + scheduler.addTask(std::bind(highPriorityTask, std::ref(robot), std::ref(log), + std::ref(robotStates), std::ref(initPose)), "HP periodic", 1, scheduler.maxPriority()); - // Start all added tasks, not blocking - scheduler.start(false); + // Start all added tasks + scheduler.start(); // Use std::thread for logging task without strict chronology std::thread lowPriorityThread(lowPriorityTask); diff --git a/test/test_loop_latency.cpp b/test/test_loop_latency.cpp index 60748f39..62337a5e 100644 --- a/test/test_loop_latency.cpp +++ b/test/test_loop_latency.cpp @@ -15,7 +15,7 @@ #include #include -#include +#include // Standard input/output definitions #include @@ -29,13 +29,15 @@ #include namespace { - -// file descriptor of the serial port +/** file descriptor of the serial port */ int g_fd = 0; + +/** Atomic signal to stop scheduler tasks */ +std::atomic g_schedStop = {false}; } // callback function for realtime periodic task -void periodicTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv::Log& log) +void periodicTask(flexiv::Robot& robot, flexiv::Log& log) { // Loop counter static unsigned int loopCounter = 0; @@ -57,7 +59,7 @@ void periodicTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv::Lo } case 1: { // signal robot server's digital out port - robot.writeDigitalOutput(0, true); + robot.writeDigitalOutput(std::vector {0}, std::vector {true}); // signal workstation PC's serial port auto n = write(g_fd, "0", 1); @@ -69,7 +71,7 @@ void periodicTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv::Lo } case 900: { // reset digital out after a few seconds - robot.writeDigitalOutput(0, false); + robot.writeDigitalOutput(std::vector {0}, std::vector {false}); break; } default: @@ -79,7 +81,7 @@ void periodicTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv::Lo } catch (const flexiv::Exception& e) { log.error(e.what()); - scheduler.stop(); + g_schedStop = true; } } @@ -142,15 +144,8 @@ int main(int argc, char* argv[]) robot.enable(); // Wait for the robot to become operational - int secondsWaited = 0; while (!robot.isOperational()) { std::this_thread::sleep_for(std::chrono::seconds(1)); - if (++secondsWaited == 10) { - log.warn( - "Still waiting for robot to become operational, please " - "check that the robot 1) has no fault, 2) is booted " - "into Auto mode"); - } } log.info("Robot is now operational"); @@ -170,12 +165,21 @@ int main(int argc, char* argv[]) //============================================================================= flexiv::Scheduler scheduler; // Add periodic task with 1ms interval and highest applicable priority - scheduler.addTask( - std::bind(periodicTask, std::ref(robot), std::ref(scheduler), std::ref(log)), - "HP periodic", 1, scheduler.maxPriority()); - // Start all added tasks, this is by default a blocking method + scheduler.addTask(std::bind(periodicTask, std::ref(robot), std::ref(log)), "HP periodic", 1, + scheduler.maxPriority()); + // Start all added tasks scheduler.start(); + // Block and wait for signal to stop scheduler tasks + while (!g_schedStop) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + // Received signal to stop scheduler tasks + scheduler.stop(); + + // Wait a bit for any last-second robot log message to arrive and get printed + std::this_thread::sleep_for(std::chrono::seconds(2)); + } catch (const flexiv::Exception& e) { log.error(e.what()); return 1; diff --git a/test/test_scheduler.cpp b/test/test_scheduler.cpp index 5a7583db..16533b25 100644 --- a/test/test_scheduler.cpp +++ b/test/test_scheduler.cpp @@ -14,9 +14,9 @@ #include #include #include +#include namespace { - /** Data shared between threads */ struct SharedData { @@ -26,10 +26,12 @@ struct SharedData /** Mutex on the shared data */ std::mutex g_mutex; +/** Atomic signal to stop scheduler tasks */ +std::atomic g_schedStop = {false}; } /** User-defined high-priority periodic task @ 1kHz */ -void highPriorityTask(flexiv::Scheduler& scheduler, flexiv::Log& log) +void highPriorityTask(flexiv::Log& log) { static unsigned int loopCounter = 0; @@ -53,7 +55,7 @@ void highPriorityTask(flexiv::Scheduler& scheduler, flexiv::Log& log) // Stop scheduler after 5 seconds if (++loopCounter > 5000) { loopCounter = 0; - scheduler.stop(); + g_schedStop = true; } // Mark loop interval start point @@ -61,7 +63,7 @@ void highPriorityTask(flexiv::Scheduler& scheduler, flexiv::Log& log) } catch (const flexiv::Exception& e) { log.error(e.what()); - scheduler.stop(); + g_schedStop = true; } } @@ -115,18 +117,33 @@ int main(int argc, char* argv[]) //============================================================================= flexiv::Scheduler scheduler; // Add periodic task with 1ms interval and highest applicable priority - scheduler.addTask(std::bind(highPriorityTask, std::ref(scheduler), std::ref(log)), - "HP periodic", 1, scheduler.maxPriority()); + scheduler.addTask( + std::bind(highPriorityTask, std::ref(log)), "HP periodic", 1, scheduler.maxPriority()); // Add periodic task with 1s interval and lowest applicable priority scheduler.addTask(std::bind(lowPriorityTask, std::ref(log)), "LP periodic", 1000, 0); - // Start all added tasks, this is by default a blocking method + // Start all added tasks scheduler.start(); + // Block and wait for signal to stop scheduler tasks + while (!g_schedStop) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + // Received signal to stop scheduler tasks + scheduler.stop(); + // Restart scheduler after 2 seconds log.warn("Scheduler will restart in 2 seconds"); std::this_thread::sleep_for(std::chrono::seconds(2)); + g_schedStop = false; scheduler.start(); + // Wait for signal to stop scheduler tasks + while (!g_schedStop) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + // Received signal to stop scheduler tasks, flexiv::Scheduler's destructor can also do the + // thread exit and resources cleanup + } catch (const flexiv::Exception& e) { log.error(e.what()); return 1; diff --git a/test/test_timeliness_monitor.cpp b/test/test_timeliness_monitor.cpp index 0f8d09f3..3124bc3b 100644 --- a/test/test_timeliness_monitor.cpp +++ b/test/test_timeliness_monitor.cpp @@ -20,63 +20,29 @@ #include #include #include +#include namespace { -// robot DOF -const int k_robotDofs = 7; - -// joint impedance control gains -const std::vector k_impedanceKp = {3000.0, 3000.0, 800.0, 800.0, 200.0, 200.0, 200.0}; -const std::vector k_impedanceKd = {80.0, 80.0, 40.0, 40.0, 8.0, 8.0, 8.0}; - +/** Atomic signal to stop scheduler tasks */ +std::atomic g_schedStop = {false}; } // callback function for realtime periodic task -void periodicTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv::Log& log, - flexiv::RobotStates& robotStates) +void periodicTask(flexiv::Robot& robot, flexiv::Log& log, const std::vector& initPos) { // Loop counter static unsigned int loopCounter = 0; - // Whether initial joint position is set - static bool isInitPositionSet = false; - - // Initial position of robot joints - static std::vector initPosition; - try { - // Monitor fault on robot server - if (robot.isFault()) { - throw flexiv::ServerException( - "periodicTask: Fault occurred on robot server, exiting ..."); - } - - // Read robot states - robot.getRobotStates(robotStates); - - // Set initial joint position - if (!isInitPositionSet) { - // check vector size before saving - if (robotStates.q.size() == k_robotDofs) { - initPosition = robotStates.q; - isInitPositionSet = true; - } + // Monitor disconnection with robot server + if (!robot.isConnected()) { + throw flexiv::ServerException("periodicTask: Robot server disconnected, exiting ..."); } - // Run control after init position is set - else { - // initialize target position to hold position - auto targetPosition = initPosition; - - std::vector torqueDesired(k_robotDofs); - // impedance control on all joints - for (size_t i = 0; i < k_robotDofs; ++i) { - torqueDesired[i] = k_impedanceKp[i] * (targetPosition[i] - robotStates.q[i]) - - k_impedanceKd[i] * robotStates.dtheta[i]; - } - // send target joint torque to RDK server - robot.streamJointTorque(torqueDesired, true); - } + // Initialize target vectors to hold position + std::vector targetVel(initPos.size(), 0); + std::vector targetAcc(initPos.size(), 0); + robot.streamJointPosition(initPos, targetVel, targetAcc); if (loopCounter == 5000) { log.warn(">>>>> Adding simulated loop delay <<<<<"); @@ -90,7 +56,7 @@ void periodicTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv::Lo } catch (const flexiv::Exception& e) { log.error(e.what()); - scheduler.stop(); + g_schedStop = true; } } @@ -151,35 +117,40 @@ int main(int argc, char* argv[]) robot.enable(); // Wait for the robot to become operational - int secondsWaited = 0; while (!robot.isOperational()) { std::this_thread::sleep_for(std::chrono::seconds(1)); - if (++secondsWaited == 10) { - log.warn( - "Still waiting for robot to become operational, please " - "check that the robot 1) has no fault, 2) is booted " - "into Auto mode"); - } } log.info("Robot is now operational"); // set mode after robot is operational - robot.setMode(flexiv::Mode::RT_JOINT_TORQUE); + robot.setMode(flexiv::Mode::RT_JOINT_POSITION); + + // Set initial joint positions + robot.getRobotStates(robotStates); + auto initPos = robotStates.q; - log.warn( - ">>>>> Simulated loop delay will be added after 5 " - "seconds <<<<<"); + log.warn(">>>>> Simulated loop delay will be added after 5 seconds <<<<<"); // Periodic Tasks //============================================================================= flexiv::Scheduler scheduler; // Add periodic task with 1ms interval and highest applicable priority - scheduler.addTask(std::bind(periodicTask, std::ref(robot), std::ref(scheduler), - std::ref(log), std::ref(robotStates)), + scheduler.addTask( + std::bind(periodicTask, std::ref(robot), std::ref(log), std::ref(initPos)), "HP periodic", 1, scheduler.maxPriority()); - // Start all added tasks, this is by default a blocking method + // Start all added tasks scheduler.start(); + // Block and wait for signal to stop scheduler tasks + while (!g_schedStop) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + // Received signal to stop scheduler tasks + scheduler.stop(); + + // Wait a bit for any last-second robot log message to arrive and get printed + std::this_thread::sleep_for(std::chrono::seconds(2)); + } catch (const flexiv::Exception& e) { log.error(e.what()); return 1;