diff --git a/README.md b/README.md index eebde490..a2755eee 100644 --- a/README.md +++ b/README.md @@ -25,52 +25,58 @@ Flexiv RDK (Robotic Development Kit), a key component of the Flexiv Robotic Soft The C++ interface of Flexiv RDK is packed into a unified modern CMake library named ``flexiv_rdk``, which can be configured via CMake on all supported OS. +NOTE: if you will only be using Python RDK, you can skip this section and jump to [Python RDK](#python-rdk). + #### Compile and install for Linux -1. In a new Terminal, use ``cmake-gui`` to configure the top-level ``flexiv_rdk`` CMake project: +1. In a new Terminal, install C++ compiler, Git, and CMake (with GUI) using the package manager: + + sudo apt install build-essential git cmake cmake-qt-gui -y + +2. Choose a directory for installing ``flexiv_rdk`` library and all its dependencies. For example, a new folder named ``rdk_install`` under the home directory. +3. In a new Terminal, use CMake to configure ``flexiv_rdk``: cd flexiv_rdk mkdir build && cd build - cmake-gui .. & + cmake .. -DCMAKE_INSTALL_PREFIX=~/rdk_install -2. *Configure* with default native compiler settings. -3. Set ``CMAKE_INSTALL_PREFIX`` to a dedicated directory (preferably not a system path) for the ``flexiv_rdk`` library to be installed to. For example ``~/rdk_install``. -4. *Configure* and *Generate*, then back to the Terminal to compile and install: + NOTE: ``-D`` followed by ``CMAKE_INSTALL_PREFIX`` is a CMake parameter specifying the path of the chosen installation directory. Alternatively, this configuration step can also be done through CMake GUI. + +4. Compile and install ``flexiv_rdk`` library: cd flexiv_rdk/build cmake --build . --target install --config Release -5. The user project can now find and link to the installed ``flexiv_rdk`` library: + NOTE: the installation of ``flexiv_rdk`` library is complete now. The following steps show how to link to the installed library from a user project. + +5. To find and link to the installed ``flexiv_rdk`` library from a user project, using the provided example project for instance: cd flexiv_rdk/example mkdir build && cd build cmake .. -DCMAKE_INSTALL_PREFIX=~/rdk_install - cmake --build . --config Release + cmake --build . --config Release -j 4 + + NOTE: ``-D`` followed by ``CMAKE_INSTALL_PREFIX`` tells user project's CMake where to find the installed ``flexiv_rdk`` library. - Note: ``CMAKE_INSTALL_PREFIX`` is set to the same directory where ``flexiv_rdk`` was installed to. 6. Assuming the system setup detailed in the Flexiv RDK Manual is done, to run an compiled example program: ./ [robot_ip] [local_ip] [...] - Note: ``sudo`` is not required unless prompted by the program. + Note: ``sudo`` is not required unless prompted by the program saying "root privilege is required". -#### Compile and install for Mac +#### Compile and install for macOS 1. In a Terminal, use ``xcode-select`` command to invoke the installation of Xcode Command Line Tools, then follow the prompted window to finish the installation. -2. Download ``cmake-3.x.x-macos-universal.dmg`` from [CMake download page](https://cmake.org/download/) and install the dmg file. The minimum required version is 3.4. +2. Download ``cmake-3.x.x-macos-universal.dmg`` from [CMake download page](https://cmake.org/download/) and install the dmg file. The minimum required version is 3.16.3. 3. When done, start CMake from Launchpad and navigate to Tools -> How to Install For Command Line Use. Then follow the instruction "Or, to install symlinks to '/usr/local/bin', run:" to install ``cmake`` and ``cmake-gui`` command for use in Terminal. -4. The rest steps are the same as [Compile and install for Linux](#compile-and-install-for-linux). +4. The rest steps are the same as [Compile and install for Linux](#compile-and-install-for-linux), beginning from step 2. #### Compile and install for Windows -1. Install any edition of Microsoft Visual Studio with version 2010 or above. Choose the "Desktop development with C++" package during installation. -2. Download ``cmake-3.x.x-windows-x86_64.msi`` from [CMake download page](https://cmake.org/download/) and install the msi file. The minimum required version is 3.4. **Add CMake to system PATH** when prompted, so that ``cmake`` and ``cmake-gui`` command can be used from Command Prompt or PowerShell. -3. Configure the ``flexiv_rdk`` CMake project using the same steps mentioned in [Compile and install for Linux](#compile-and-install-for-linux). -4. The rest steps are the same as [Compile and install for Linux](#compile-and-install-for-linux). -5. To run an compiled example program: - - cd Release - .exe [robot_ip] [local_ip] [...] +1. Install Microsoft Visual Studio with version 2015 or above (MSVC 14.0+). Choose the "Desktop development with C++" package during installation. +2. Download ``cmake-3.x.x-windows-x86_64.msi`` from [CMake download page](https://cmake.org/download/) and install the msi file. The minimum required version is 3.16.3. **Add CMake to system PATH** when prompted, so that ``cmake`` and ``cmake-gui`` command can be used from Command Prompt or a bash emulator. +3. Install a bash emulator. Git Bash that comes with Git (for Windows) installation is recommended. +4. Within the bash emulator, the rest steps are the same as [Compile and install for Linux](#compile-and-install-for-linux), beginning from step 2. ### Python RDK diff --git a/example/CMakeLists.txt b/example/CMakeLists.txt index 5c631209..836f81b7 100644 --- a/example/CMakeLists.txt +++ b/example/CMakeLists.txt @@ -14,25 +14,26 @@ endif() # Minumum example list for all OS set(EXAMPLE_LIST - auto_recovery - clear_fault - display_robot_states - gripper_control - plan_execution - primitive_execution - teach_by_demonstration + basics1_display_robot_states + basics2_clear_fault + basics3_primitive_execution + basics4_plan_execution + basics5_zero_force_torque_sensors + basics6_gripper_control + basics7_auto_recovery + intermediate7_teach_by_demonstration ) # Additional examples for Linux and Mac if(CMAKE_HOST_UNIX) list(APPEND EXAMPLE_LIST - RT_cartesian_pure_motion_control - RT_cartesian_motion_force_control - RT_joint_floating - RT_joint_impedance_control - RT_joint_position_control - robot_dynamics + intermediate1_realtime_joint_position_control + intermediate2_realtime_joint_torque_control + intermediate3_realtime_joint_floating + intermediate4_realtime_cartesian_pure_motion_control + intermediate5_realtime_cartesian_motion_force_control + intermediate6_robot_dynamics ) endif() diff --git a/example/display_robot_states.cpp b/example/basics1_display_robot_states.cpp similarity index 71% rename from example/display_robot_states.cpp rename to example/basics1_display_robot_states.cpp index 74fc5a8b..da0466e6 100644 --- a/example/display_robot_states.cpp +++ b/example/basics1_display_robot_states.cpp @@ -1,6 +1,7 @@ /** - * @example display_robot_states.cpp - * Print received robot states. + * @example basics1_display_robot_states.cpp + * This tutorial does the very first thing: check connection with the robot server and print + * received robot states. * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ @@ -13,7 +14,28 @@ #include #include -/** Print robot states data @ 1Hz */ +/** @brief Print tutorial description */ +void printDescription() +{ + std::cout << "This tutorial does the very first thing: check connection with the robot server " + "and print received robot states." + << std::endl + << std::endl; +} + +/** @brief Print program usage help */ +void printHelp() +{ + // clang-format off + 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: None" << std::endl; + std::cout << std::endl; + // clang-format on +} + +/** @brief Print robot states data @ 1Hz */ void printRobotStates(flexiv::Robot& robot, flexiv::Log& log) { // Data struct storing robot states @@ -31,38 +53,30 @@ void printRobotStates(flexiv::Robot& robot, flexiv::Log& log) } } -void printHelp() -{ - // clang-format off - 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: None" << std::endl; - std::cout << std::endl; - // clang-format on -} - int main(int argc, char* argv[]) { - // Log object for printing message with timestamp and coloring + // Program Setup + // ============================================================================================= + // Logger for printing message with timestamp and coloring flexiv::Log log; - // Parse Parameters - //============================================================================= + // Parse parameters if (argc < 3 || flexiv::utility::programArgsExistAny(argc, argv, {"-h", "--help"})) { printHelp(); return 1; } - // IP of the robot server std::string robotIP = argv[1]; - // IP of the workstation PC running this program std::string localIP = argv[2]; + // Print description + log.info("Tutorial description:"); + printDescription(); + try { // RDK Initialization - //============================================================================= + // ========================================================================================= // Instantiate robot interface flexiv::Robot robot(robotIP, localIP); @@ -90,17 +104,16 @@ int main(int argc, char* argv[]) 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"); + "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"); - // Periodic Tasks - //============================================================================= - // Use std::thread to do scheduling so that this example can run on all - // OS, since not all OS support flexiv::Scheduler + // Print States + // ========================================================================================= + // Use std::thread to do scheduling so that this example can run on all OS, since not all OS + // support flexiv::Scheduler std::thread lowPriorityThread(std::bind(printRobotStates, std::ref(robot), std::ref(log))); // Properly exit thread diff --git a/example/clear_fault.cpp b/example/basics2_clear_fault.cpp similarity index 61% rename from example/clear_fault.cpp rename to example/basics2_clear_fault.cpp index cd39048a..e2337b23 100644 --- a/example/clear_fault.cpp +++ b/example/basics2_clear_fault.cpp @@ -1,6 +1,7 @@ /** - * @example clear_fault.cpp - * Clear minor fault on the robot server side if any. + * @example basics2_clear_fault.cpp + * This tutorial clears minor faults from the robot server if any. Note that critical faults cannot + * be cleared, see RDK manual for more details. * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ @@ -14,6 +15,16 @@ #include #include +/** @brief Print tutorial description */ +void printDescription() +{ + std::cout << "This tutorial clears minor faults from the robot server if any. Note that " + "critical faults cannot be cleared, see RDK manual for more details." + << std::endl + << std::endl; +} + +/** @brief Print program usage help */ void printHelp() { // clang-format off @@ -27,31 +38,34 @@ void printHelp() int main(int argc, char* argv[]) { - // Log object for printing message with timestamp and coloring + // Program Setup + // ============================================================================================= + // Logger for printing message with timestamp and coloring flexiv::Log log; - // Parse Parameters - //============================================================================= + // Parse parameters if (argc < 3 || flexiv::utility::programArgsExistAny(argc, argv, {"-h", "--help"})) { printHelp(); return 1; } - // IP of the robot server std::string robotIP = argv[1]; - // IP of the workstation PC running this program std::string localIP = argv[2]; + // Print description + log.info("Tutorial description:"); + printDescription(); + try { // RDK Initialization - //============================================================================= + // ========================================================================================= // Instantiate robot interface flexiv::Robot robot(robotIP, localIP); - // Application-specific Code - //============================================================================= - // Clear fault on robot server if any + // Fault Clearing + // ========================================================================================= + // Check if the robot has fault if (robot.isFault()) { log.warn("Fault occurred on robot server, trying to clear ..."); // Try to clear the fault diff --git a/example/primitive_execution.cpp b/example/basics3_primitive_execution.cpp similarity index 65% rename from example/primitive_execution.cpp rename to example/basics3_primitive_execution.cpp index 2d769d9c..380ea48c 100644 --- a/example/primitive_execution.cpp +++ b/example/basics3_primitive_execution.cpp @@ -1,8 +1,7 @@ /** - * @example primitive_execution.cpp - * Execute various [Move] series primitives. - * @note For detailed documentation on all available primitives, please see - * [Flexiv Primitives](https://www.flexiv.com/primitives/). + * @example basics3_primitive_execution.cpp + * 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/). * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ @@ -15,6 +14,17 @@ #include #include +/** @brief Print tutorial description */ +void printDescription() +{ + std::cout << "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/)." + << std::endl + << std::endl; +} + +/** @brief Print program usage help */ void printHelp() { // clang-format off @@ -28,31 +38,31 @@ void printHelp() int main(int argc, char* argv[]) { - // Log object for printing message with timestamp and coloring + // Program Setup + // ============================================================================================= + // Logger for printing message with timestamp and coloring flexiv::Log log; - // Parse Parameters - //============================================================================= + // Parse parameters if (argc < 3 || flexiv::utility::programArgsExistAny(argc, argv, {"-h", "--help"})) { printHelp(); return 1; } - // IP of the robot server std::string robotIP = argv[1]; - // IP of the workstation PC running this program std::string localIP = argv[2]; + // Print description + log.info("Tutorial description:"); + printDescription(); + try { // RDK Initialization - //============================================================================= + // ========================================================================================= // 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 ..."); @@ -77,23 +87,21 @@ int main(int argc, char* argv[]) 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"); + "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"); - // Set mode after robot is operational + // Execute Primitives + // ========================================================================================= + // Switch to primitive execution mode robot.setMode(flexiv::Mode::NRT_PRIMITIVE_EXECUTION); - // Application-specific Code - //============================================================================= - // (1) Go to home pose - // ---------------------------------------------------------------------------- - // All parameters of the "Home" primitive are optional, - // thus we can skip the parameters and the default values will be used + // ----------------------------------------------------------------------------------------- + // All parameters of the "Home" primitive are optional, thus we can skip the parameters and + // the default values will be used log.info("Executing primitive: Home"); // Send command to robot @@ -105,9 +113,8 @@ int main(int argc, char* argv[]) } // (2) Move robot joints to target positions - // ---------------------------------------------------------------------------- - // The required parameter takes in 7 target joint positions - // Unit: degrees + // ----------------------------------------------------------------------------------------- + // The required parameter takes in 7 target joint positions. Unit: degrees log.info("Executing primitive: MoveJ"); // Send command to robot @@ -119,51 +126,45 @@ int main(int argc, char* argv[]) } // (3) Move robot TCP to a target position in world (base) frame - // ---------------------------------------------------------------------------- + // ----------------------------------------------------------------------------------------- // Required parameter: // target: final target position // [pos_x pos_y pos_z rot_x rot_y rot_z ref_frame ref_point] - // unit: m, deg + // Unit: m, deg // Optional parameter: // waypoints: waypoints to pass before reaching final target - // [same format as above, but can repeat for number of waypoints] + // (same format as above, but can repeat for number of waypoints) // maxVel: maximum TCP linear velocity - // unit: m/s - // NOTE: The rotations are using Euler ZYX convention, rot_x means Euler - // ZYX angle around X axis + // Unit: m/s + // NOTE: The rotations use Euler ZYX convention, rot_x means Euler ZYX angle around X axis log.info("Executing primitive: MoveL"); // 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)"); - - // 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. + "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)"); + + // 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 (flexiv::utility::parsePtStates(robot.getPrimitiveStates(), "reachedTarget") != "1") { std::this_thread::sleep_for(std::chrono::seconds(1)); } // (4) Another MoveL that uses TCP frame - // ---------------------------------------------------------------------------- - // In this example the reference frame is changed from - // WORLD::WORLD_ORIGIN to TRAJ::START, which represents the current TCP - // frame + // ----------------------------------------------------------------------------------------- + // In this example the reference frame is changed from WORLD::WORLD_ORIGIN to TRAJ::START, + // which represents the current TCP frame log.info("Executing primitive: MoveL"); - // Example to convert target quaternion [w,x,y,z] to Euler ZYX using - // utility functions + // Example to convert target quaternion [w,x,y,z] to Euler ZYX using utility functions std::vector targetQuat = {0.9185587, 0.1767767, 0.3061862, 0.1767767}; // ZYX = [30, 30, 30] degrees auto targetEulerDeg = flexiv::utility::rad2Deg(flexiv::utility::quat2EulerZYX(targetQuat)); - // Send command to robot. This motion will hold current TCP position and - // only do TCP rotation + // 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 " + flexiv::utility::vec2Str(targetEulerDeg) + "TRAJ START)"); diff --git a/example/plan_execution.cpp b/example/basics4_plan_execution.cpp similarity index 74% rename from example/plan_execution.cpp rename to example/basics4_plan_execution.cpp index 291e0e7d..5300ba6c 100644 --- a/example/plan_execution.cpp +++ b/example/basics4_plan_execution.cpp @@ -1,6 +1,9 @@ /** - * @example plan_execution.cpp - * Select a plan from a list to execute using RDK's plan execution API. + * @example basics4_plan_execution.cpp + * 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. * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ @@ -13,6 +16,20 @@ #include #include +/** @brief Print tutorial description */ +void printDescription() +{ + std::cout << "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." + << std::endl + << std::endl; +} + +/** @brief Print program usage help */ + void printHelp() { // clang-format off @@ -26,25 +43,28 @@ void printHelp() int main(int argc, char* argv[]) { - // Log object for printing message with timestamp and coloring + // Program Setup + // ============================================================================================= + // Logger for printing message with timestamp and coloring flexiv::Log log; - // Parse Parameters - //============================================================================= + // Parse parameters if (argc < 3 || flexiv::utility::programArgsExistAny(argc, argv, {"-h", "--help"})) { printHelp(); return 1; } - // IP of the robot server std::string robotIP = argv[1]; - // IP of the workstation PC running this program std::string localIP = argv[2]; + // Print description + log.info("Tutorial description:"); + printDescription(); + try { // RDK Initialization - //============================================================================= + // ========================================================================================= // Instantiate robot interface flexiv::Robot robot(robotIP, localIP); @@ -72,18 +92,17 @@ int main(int argc, char* argv[]) 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"); + "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"); - // Set mode after robot is operational + // Execute Plans + // ========================================================================================= + // Switch to plan execution mode robot.setMode(flexiv::Mode::NRT_PLAN_EXECUTION); - // Application-specific Code - //============================================================================= // Plan info data flexiv::PlanInfo planInfo; diff --git a/example/basics5_zero_force_torque_sensors.cpp b/example/basics5_zero_force_torque_sensors.cpp new file mode 100644 index 00000000..64a60195 --- /dev/null +++ b/example/basics5_zero_force_torque_sensors.cpp @@ -0,0 +1,129 @@ +/** + * @example basics5_zero_force_torque_sensors.cpp + * 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. + * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. + * @author Flexiv + */ + +#include +#include +#include +#include + +#include +#include + +/** @brief Print tutorial description */ +void printDescription() +{ + std::cout << "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." + << std::endl + << std::endl; +} + +/** @brief Print program usage help */ +void printHelp() +{ + // clang-format off + 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: None" << std::endl; + std::cout << std::endl; + // clang-format on +} + +int main(int argc, char* argv[]) +{ + // Program Setup + // ============================================================================================= + // Logger for printing message with timestamp and coloring + flexiv::Log log; + + // Parse parameters + if (argc < 3 || flexiv::utility::programArgsExistAny(argc, argv, {"-h", "--help"})) { + printHelp(); + return 1; + } + // IP of the robot server + std::string robotIP = argv[1]; + // IP of the workstation PC running this program + std::string localIP = argv[2]; + + // Print description + log.info("Tutorial description:"); + printDescription(); + + try { + // RDK Initialization + // ========================================================================================= + // Instantiate robot interface + flexiv::Robot robot(robotIP, localIP); + + // Clear fault on robot server if any + if (robot.isFault()) { + log.warn("Fault occurred on robot server, trying to clear ..."); + // Try to clear the fault + robot.clearFault(); + std::this_thread::sleep_for(std::chrono::seconds(2)); + // Check again + if (robot.isFault()) { + log.error("Fault cannot be cleared, exiting ..."); + return 1; + } + log.info("Fault on robot server is cleared"); + } + + // Enable the robot, make sure the E-stop is released before enabling + log.info("Enabling robot ..."); + 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"); + + // Zero Sensors + // ========================================================================================= + // Get and print the current TCP force/moment readings + flexiv::RobotStates robotStates; + robot.getRobotStates(robotStates); + log.info("TCP force and moment reading in base frame BEFORE sensor zeroing: " + + flexiv::utility::vec2Str(robotStates.extWrenchInBase) + "[N][Nm]"); + + // Run the "ZeroFTSensor" primitive to automatically zero force and torque sensors + robot.setMode(flexiv::Mode::NRT_PRIMITIVE_EXECUTION); + 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"); + + // Wait for primitive completion + while (robot.isBusy()) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + log.info("Sensor zeroing complete"); + + // Get and print the current TCP force/moment readings + robot.getRobotStates(robotStates); + log.info("TCP force and moment reading in base frame AFTER sensor zeroing: " + + flexiv::utility::vec2Str(robotStates.extWrenchInBase) + "[N][Nm]"); + + } catch (const flexiv::Exception& e) { + log.error(e.what()); + return 1; + } + + return 0; +} diff --git a/example/gripper_control.cpp b/example/basics6_gripper_control.cpp similarity index 78% rename from example/gripper_control.cpp rename to example/basics6_gripper_control.cpp index a10ff7b6..b5c1d8b7 100644 --- a/example/gripper_control.cpp +++ b/example/basics6_gripper_control.cpp @@ -1,6 +1,6 @@ /** - * @example gripper_control.cpp - * Position and force control with grippers supported by Flexiv. + * @example basics6_gripper_control.cpp + * This tutorial does position and force control (if available) for grippers supported by Flexiv. * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ @@ -21,7 +21,28 @@ namespace { std::atomic g_isDone = {false}; } -/** Print gripper states data @ 1Hz */ +/** @brief Print tutorial description */ +void printDescription() +{ + std::cout << "This tutorial does position and force control (if available) for grippers " + "supported by Flexiv." + << std::endl + << std::endl; +} + +/** @brief Print program usage help */ +void printHelp() +{ + // clang-format off + 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: None" << std::endl; + std::cout << std::endl; + // clang-format on +} + +/** @brief Print gripper states data @ 1Hz */ void printGripperStates(flexiv::Gripper& gripper, flexiv::Log& log) { // Data struct storing gripper states @@ -39,38 +60,30 @@ void printGripperStates(flexiv::Gripper& gripper, flexiv::Log& log) } } -void printHelp() -{ - // clang-format off - 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: None" << std::endl; - std::cout << std::endl; - // clang-format on -} - int main(int argc, char* argv[]) { - // Log object for printing message with timestamp and coloring + // Program Setup + // ============================================================================================= + // Logger for printing message with timestamp and coloring flexiv::Log log; - // Parse Parameters - //============================================================================= + // Parse parameters if (argc < 3 || flexiv::utility::programArgsExistAny(argc, argv, {"-h", "--help"})) { printHelp(); return 1; } - // IP of the robot server std::string robotIP = argv[1]; - // IP of the workstation PC running this program std::string localIP = argv[2]; + // Print description + log.info("Tutorial description:"); + printDescription(); + try { // RDK Initialization - //============================================================================= + // ========================================================================================= // Instantiate robot interface flexiv::Robot robot(robotIP, localIP); @@ -98,28 +111,27 @@ int main(int argc, char* argv[]) 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"); + "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"); - // Gripper control is not available if the robot is in IDLE mode, so put - // robot into some mode other than IDLE + // Gripper Control + // ========================================================================================= + // Gripper control is not available if the robot is in IDLE mode, so switch to some mode + // other than IDLE robot.setMode(flexiv::Mode::NRT_PLAN_EXECUTION); robot.executePlan("PLAN-Home"); std::this_thread::sleep_for(std::chrono::seconds(1)); - // Application-specific Code - //============================================================================= - // Instantiate gripper + // Instantiate gripper control interface flexiv::Gripper gripper(robot); // Thread for printing gripper states std::thread printThread(printGripperStates, std::ref(gripper), std::ref(log)); - // Position control test + // Position control log.info("Closing gripper"); gripper.move(0.01, 0.1, 20); std::this_thread::sleep_for(std::chrono::seconds(2)); @@ -127,7 +139,7 @@ int main(int argc, char* argv[]) gripper.move(0.09, 0.1, 20); std::this_thread::sleep_for(std::chrono::seconds(2)); - // Stop test + // Stop log.info("Closing gripper"); gripper.move(0.01, 0.1, 20); std::this_thread::sleep_for(std::chrono::milliseconds(500)); @@ -144,7 +156,7 @@ int main(int argc, char* argv[]) gripper.stop(); std::this_thread::sleep_for(std::chrono::seconds(2)); - // Force control test, if available + // Force control, if available (sensed force is not zero) flexiv::GripperStates gripperStates; gripper.getGripperStates(gripperStates); if (fabs(gripperStates.force) > std::numeric_limits::epsilon()) { diff --git a/example/auto_recovery.cpp b/example/basics7_auto_recovery.cpp similarity index 57% rename from example/auto_recovery.cpp rename to example/basics7_auto_recovery.cpp index 426c555c..1a68da39 100644 --- a/example/auto_recovery.cpp +++ b/example/basics7_auto_recovery.cpp @@ -1,9 +1,7 @@ /** - * @example auto_recovery.cpp - * Run auto-recovery if the robot's safety system is in recovery state, - * otherwise run damped floating with joint soft limit disabled, so that the - * user can manually trigger a safety system recovery state by moving any joint - * close to its limit. See flexiv::Robot::isRecoveryState() for more details. + * @example basics7_auto_recovery.cpp + * 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. * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ @@ -17,6 +15,17 @@ #include #include +/** @brief Print tutorial description */ +void printDescription() +{ + std::cout + << "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." + << std::endl + << std::endl; +} + +/** @brief Print program usage help */ void printHelp() { // clang-format off @@ -30,57 +39,53 @@ void printHelp() int main(int argc, char* argv[]) { - // Log object for printing message with timestamp and coloring + // Program Setup + // ============================================================================================= + // Logger for printing message with timestamp and coloring flexiv::Log log; - // Parse Parameters - //============================================================================= + // Parse parameters if (argc < 3 || flexiv::utility::programArgsExistAny(argc, argv, {"-h", "--help"})) { printHelp(); return 1; } - // IP of the robot server std::string robotIP = argv[1]; - // IP of the workstation PC running this program std::string localIP = argv[2]; + // Print description + log.info("Tutorial description:"); + printDescription(); + try { // RDK Initialization - //============================================================================= + // ========================================================================================= // Instantiate robot interface flexiv::Robot robot(robotIP, localIP); - // Create data struct for storing robot states - flexiv::RobotStates robotStates; - // Enable the robot, make sure the E-stop is released before enabling log.info("Enabling robot ..."); robot.enable(); - // Application-specific Code - //============================================================================= - // If the system is in recovery state, we can't use isOperational to - // tell if the enabling process is done, so just wait long enough for - // the process to finish + // Run Auto-recovery + // ========================================================================================= + // If the system is in recovery state, we can't use isOperational to tell if the enabling + // 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 move back into allowed position range + // Start 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 auto recovery is done + // 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)); } } // Otherwise the system is normal, do nothing else { - log.info( - "Robot system is not in recovery state, nothing to be done, " - "exiting ..."); + log.info("Robot system is not in recovery state, nothing to be done, exiting ..."); } } catch (const flexiv::Exception& e) { log.error(e.what()); diff --git a/example/RT_joint_position_control.cpp b/example/intermediate1_realtime_joint_position_control.cpp similarity index 76% rename from example/RT_joint_position_control.cpp rename to example/intermediate1_realtime_joint_position_control.cpp index e617a057..b4fe3224 100644 --- a/example/RT_joint_position_control.cpp +++ b/example/intermediate1_realtime_joint_position_control.cpp @@ -1,6 +1,6 @@ /** - * @example RT_joint_position_control.cpp - * Real-time joint position control to hold or sine-sweep all joints. + * @example intermediate1_realtime_joint_position_control.cpp + * This tutorial runs real-time joint position control to hold or sine-sweep all robot joints. * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ @@ -25,7 +25,29 @@ constexpr double k_sineAmp = 0.035; constexpr double k_sineFreq = 0.3; } -/** Callback function for realtime periodic task */ +/** @brief Print tutorial description */ +void printDescription() +{ + std::cout << "This tutorial runs real-time joint position control to hold or sine-sweep all " + "robot joints." + << std::endl + << std::endl; +} + +/** @brief Print program usage help */ +void printHelp() +{ + // clang-format off + 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]" << std::endl; + std::cout << " --hold: robot holds current joint positions, otherwise do a sine-sweep" << std::endl; + std::cout << std::endl; + // clang-format on +} + +/** @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) @@ -62,8 +84,7 @@ void periodicTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv::Lo } } else { throw flexiv::InputException( - "periodicTask: unknown motion type. Accepted motion types: " - "hold, sine-sweep"); + "periodicTask: unknown motion type. Accepted motion types: hold, sine-sweep"); } // Send target joint position to RDK server @@ -77,36 +98,27 @@ void periodicTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv::Lo } } -void printHelp() -{ - // clang-format off - 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]" << std::endl; - std::cout << " --hold: robot holds current joint positions, otherwise do a sine-sweep" << std::endl; - std::cout << std::endl; - // clang-format on -} - int main(int argc, char* argv[]) { - // Log object for printing message with timestamp and coloring + // Program Setup + // ============================================================================================= + // Logger for printing message with timestamp and coloring flexiv::Log log; - // Parse Parameters - //============================================================================= + // Parse parameters if (argc < 3 || flexiv::utility::programArgsExistAny(argc, argv, {"-h", "--help"})) { printHelp(); return 1; } - // IP of the robot server std::string robotIP = argv[1]; - // IP of the workstation PC running this program std::string localIP = argv[2]; + // Print description + log.info("Tutorial description:"); + printDescription(); + // Type of motion specified by user std::string motionType = ""; if (flexiv::utility::programArgsExist(argc, argv, "--hold")) { @@ -119,7 +131,7 @@ int main(int argc, char* argv[]) try { // RDK Initialization - //============================================================================= + // ========================================================================================= // Instantiate robot interface flexiv::Robot robot(robotIP, localIP); @@ -150,14 +162,25 @@ int main(int argc, char* argv[]) 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"); + "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"); - // Set mode after robot is operational + // Move robot to home pose + log.info("Moving to home pose"); + robot.setMode(flexiv::Mode::NRT_PRIMITIVE_EXECUTION); + robot.executePrimitive("Home()"); + + // Wait for the primitive to finish + while (robot.isBusy()) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + + // Real-time Joint Position Control + // ========================================================================================= + // Switch to real-time joint position control mode robot.setMode(flexiv::Mode::RT_JOINT_POSITION); // Set initial joint positions @@ -165,8 +188,7 @@ int main(int argc, char* argv[]) auto initPos = robotStates.q; log.info("Initial joint positions set to: " + flexiv::utility::vec2Str(initPos)); - // Periodic Tasks - //============================================================================= + // Create real-time scheduler to run periodic tasks flexiv::Scheduler scheduler; // Add periodic task with 1ms interval and highest applicable priority scheduler.addTask( diff --git a/example/RT_joint_impedance_control.cpp b/example/intermediate2_realtime_joint_torque_control.cpp similarity index 73% rename from example/RT_joint_impedance_control.cpp rename to example/intermediate2_realtime_joint_torque_control.cpp index 49ac4620..e75abdca 100644 --- a/example/RT_joint_impedance_control.cpp +++ b/example/intermediate2_realtime_joint_torque_control.cpp @@ -1,6 +1,8 @@ /** - * @example RT_joint_impedance_control.cpp - * Real-time joint impedance control using joint torque streaming. + * @example intermediate2_realtime_joint_torque_control.cpp + * This tutorial runs real-time joint torque control to hold or sine-sweep all robot joints. An + * outer position loop is used to generate joint torque commands. This outer position loop + inner + * torque loop together is also known as an impedance controller. * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ @@ -20,7 +22,7 @@ namespace { /** RT loop period [sec] */ constexpr double k_loopPeriod = 0.001; -/** Joint impedance control gains */ +/** Outer position loop (impedance) gains, values are only for demo purpose */ 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}; @@ -29,7 +31,31 @@ constexpr double k_sineAmp = 0.035; constexpr double k_sineFreq = 0.3; } -/** Callback function for realtime periodic task */ +/** @brief Print tutorial description */ +void printDescription() +{ + std::cout + << "This tutorial runs real-time joint torque control to hold or sine-sweep all robot " + "joints. An outer position loop is used to generate joint torque commands. This outer " + "position loop + inner torque loop together is also known as an impedance controller." + << std::endl + << std::endl; +} + +/** @brief Print program usage help */ +void printHelp() +{ + // clang-format off + 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]" << std::endl; + std::cout << " --hold: robot holds current joint positions, otherwise do a sine-sweep" << std::endl; + std::cout << std::endl; + // clang-format on +} + +/** @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) @@ -64,8 +90,7 @@ void periodicTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv::Lo } } else { throw flexiv::InputException( - "periodicTask: unknown motion type. Accepted motion types: " - "hold, sine-sweep"); + "periodicTask: unknown motion type. Accepted motion types: hold, sine-sweep"); } // Run impedance control on all joints @@ -86,36 +111,27 @@ void periodicTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv::Lo } } -void printHelp() -{ - // clang-format off - 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]" << std::endl; - std::cout << " --hold: robot holds current joint positions, otherwise do a sine-sweep" << std::endl; - std::cout << std::endl; - // clang-format on -} - int main(int argc, char* argv[]) { - // Log object for printing message with timestamp and coloring + // Program Setup + // ============================================================================================= + // Logger for printing message with timestamp and coloring flexiv::Log log; - // Parse Parameters - //============================================================================= + // Parse parameters if (argc < 3 || flexiv::utility::programArgsExistAny(argc, argv, {"-h", "--help"})) { printHelp(); return 1; } - // IP of the robot server std::string robotIP = argv[1]; - // IP of the workstation PC running this program std::string localIP = argv[2]; + // Print description + log.info("Tutorial description:"); + printDescription(); + // Type of motion specified by user std::string motionType = ""; if (flexiv::utility::programArgsExist(argc, argv, "--hold")) { @@ -128,7 +144,7 @@ int main(int argc, char* argv[]) try { // RDK Initialization - //============================================================================= + // ========================================================================================= // Instantiate robot interface flexiv::Robot robot(robotIP, localIP); @@ -159,14 +175,25 @@ int main(int argc, char* argv[]) 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"); + "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"); - // Set mode after robot is operational + // Move robot to home pose + log.info("Moving to home pose"); + robot.setMode(flexiv::Mode::NRT_PRIMITIVE_EXECUTION); + robot.executePrimitive("Home()"); + + // Wait for the primitive to finish + while (robot.isBusy()) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + + // Real-time Joint Torque Control + // ========================================================================================= + // Switch to real-time joint torque control mode robot.setMode(flexiv::Mode::RT_JOINT_TORQUE); // Set initial joint positions @@ -174,8 +201,7 @@ int main(int argc, char* argv[]) auto initPos = robotStates.q; log.info("Initial joint positions set to: " + flexiv::utility::vec2Str(initPos)); - // Periodic Tasks - //============================================================================= + // Create real-time scheduler to run periodic tasks flexiv::Scheduler scheduler; // Add periodic task with 1ms interval and highest applicable priority scheduler.addTask( diff --git a/example/RT_joint_floating.cpp b/example/intermediate3_realtime_joint_floating.cpp similarity index 65% rename from example/RT_joint_floating.cpp rename to example/intermediate3_realtime_joint_floating.cpp index 761e0744..ffc9001f 100644 --- a/example/RT_joint_floating.cpp +++ b/example/intermediate3_realtime_joint_floating.cpp @@ -1,10 +1,9 @@ /** - * @example RT_joint_floating.cpp - * Real-time joint floating with gentle velocity damping, gravity compensation, - * and soft protection against position limits. This example is ideal for - * verifying the system's whole-loop real-timeliness, accuracy of the robot - * dynamic mode, and joint torque control performance. If everything works well, - * all joints should float smoothly. + * @example intermediate3_realtime_joint_floating.cpp + * This tutorial runs real-time joint floating with gentle velocity damping, gravity compensation, + * and soft protection against position limits. This example is ideal for verifying the system's + * whole-loop real-timeliness, accuracy of the robot dynamics model, and joint torque control + * performance. If everything works well, all joints should float smoothly. * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ @@ -20,11 +19,35 @@ #include namespace { -/** Damping gains for floating */ +/** 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}; } -/** Callback function for realtime periodic task */ +/** @brief Print tutorial description */ +void printDescription() +{ + std::cout << "This tutorial runs real-time joint floating with gentle velocity damping, " + "gravity compensation, and soft protection against position limits. This example " + "is ideal for verifying the system's whole-loop real-timeliness, accuracy of the " + "robot dynamics model, and joint torque control performance. If everything works " + "well, all joints should float smoothly." + << std::endl + << std::endl; +} + +/** @brief Print program usage help */ +void printHelp() +{ + // clang-format off + 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: None" << std::endl; + std::cout << std::endl; + // clang-format on +} + +/** @brief Callback function for realtime periodic task */ void periodicTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv::Log& log, flexiv::RobotStates& robotStates) { @@ -59,38 +82,30 @@ void periodicTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv::Lo } } -void printHelp() -{ - // clang-format off - 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: None" << std::endl; - std::cout << std::endl; - // clang-format on -} - int main(int argc, char* argv[]) { - // Log object for printing message with timestamp and coloring + // Program Setup + // ============================================================================================= + // Logger for printing message with timestamp and coloring flexiv::Log log; - // Parse Parameters - //============================================================================= + // Parse parameters if (argc < 3 || flexiv::utility::programArgsExistAny(argc, argv, {"-h", "--help"})) { printHelp(); return 1; } - // IP of the robot server std::string robotIP = argv[1]; - // IP of the workstation PC running this program std::string localIP = argv[2]; + // Print description + log.info("Tutorial description:"); + printDescription(); + try { // RDK Initialization - //============================================================================= + // ========================================================================================= // Instantiate robot interface flexiv::Robot robot(robotIP, localIP); @@ -121,18 +136,28 @@ int main(int argc, char* argv[]) 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"); + "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"); - // Set mode after robot is operational + // Move robot to home pose + log.info("Moving to home pose"); + robot.setMode(flexiv::Mode::NRT_PRIMITIVE_EXECUTION); + robot.executePrimitive("Home()"); + + // Wait for the primitive to finish + while (robot.isBusy()) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + + // Real-time Joint Floating + // ========================================================================================= + // Switch to real-time joint torque control mode robot.setMode(flexiv::Mode::RT_JOINT_TORQUE); - // Periodic Tasks - //============================================================================= + // 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), diff --git a/example/RT_cartesian_pure_motion_control.cpp b/example/intermediate4_realtime_cartesian_pure_motion_control.cpp similarity index 77% rename from example/RT_cartesian_pure_motion_control.cpp rename to example/intermediate4_realtime_cartesian_pure_motion_control.cpp index a9387625..ae4a6249 100644 --- a/example/RT_cartesian_pure_motion_control.cpp +++ b/example/intermediate4_realtime_cartesian_pure_motion_control.cpp @@ -1,6 +1,6 @@ /** - * @example RT_cartesian_pure_motion_control.cpp - * Real-time Cartesian-space pure motion control to hold or sine-sweep the robot + * @example intermediate4_realtime_cartesian_pure_motion_control.cpp + * This tutorial runs real-time Cartesian-space pure motion control to hold or sine-sweep the robot * TCP. A simple collision detection is also included. * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. * @author Flexiv @@ -29,14 +29,37 @@ constexpr double k_swingAmp = 0.1; /** TCP sine-sweep frequency [Hz] */ constexpr double k_swingFreq = 0.3; -/** External TCP force threshold for collision detection [N] */ +/** External TCP force threshold for collision detection, value is only for demo purpose [N] */ constexpr double k_extForceThreshold = 10.0; -/** External joint torque threshold for collision detection [Nm] */ +/** External joint torque threshold for collision detection, value is only for demo purpose [Nm] */ constexpr double k_extTorqueThreshold = 5.0; } -/** Callback function for realtime periodic task */ +/** @brief Print tutorial description */ +void printDescription() +{ + std::cout << "This tutorial runs real-time Cartesian-space pure motion control to hold or " + "sine-sweep the robot TCP. A simple collision detection is also included." + << std::endl + << std::endl; +} + +/** @brief Print program usage help */ +void printHelp() +{ + // clang-format off + 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 << " --hold: robot holds current TCP pose, otherwise do a sine-sweep" << std::endl; + std::cout << " --collision: enable collision detection, robot will stop upon collision" << std::endl; + std::cout << std::endl; + // clang-format on +} + +/** @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) @@ -65,8 +88,8 @@ void periodicTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv::Lo } // Otherwise robot TCP will hold at initial pose - // Send command. Calling this method with only target pose input results - // in pure motion control + // Send command. Calling this method with only target pose input results in pure motion + // control robot.streamCartesianMotionForce(targetTcpPose); // Do the following operations in sequence for every 20 seconds @@ -107,8 +130,8 @@ void periodicTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv::Lo break; } - // Simple collision detection: stop robot if collision is detected from - // either end-effector or robot body + // Simple collision detection: stop robot if collision is detected from either end-effector + // or robot body if (enableCollision) { bool collisionDetected = false; Eigen::Vector3d extForce = {robotStates.extWrenchInBase[0], @@ -137,37 +160,27 @@ void periodicTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv::Lo } } -void printHelp() -{ - // clang-format off - 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 << " --hold: robot holds current TCP pose, otherwise do a sine-sweep" << std::endl; - std::cout << " --collision: enable collision detection, robot will stop upon collision" << std::endl; - std::cout << std::endl; - // clang-format on -} - int main(int argc, char* argv[]) { - // Log object for printing message with timestamp and coloring + // Program Setup + // ============================================================================================= + // Logger for printing message with timestamp and coloring flexiv::Log log; - // Parse Parameters - //============================================================================= + // Parse parameters if (argc < 3 || flexiv::utility::programArgsExistAny(argc, argv, {"-h", "--help"})) { printHelp(); return 1; } - // IP of the robot server std::string robotIP = argv[1]; - // IP of the workstation PC running this program std::string localIP = argv[2]; + // Print description + log.info("Tutorial description:"); + printDescription(); + // Type of motion specified by user bool enableHold = false; if (flexiv::utility::programArgsExist(argc, argv, "--hold")) { @@ -188,7 +201,7 @@ int main(int argc, char* argv[]) try { // RDK Initialization - //============================================================================= + // ========================================================================================= // Instantiate robot interface flexiv::Robot robot(robotIP, localIP); @@ -219,23 +232,36 @@ int main(int argc, char* argv[]) 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"); + "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"); - // IMPORTANT: must calibrate force/torque sensor for accurate collision - // detection + // Move robot to home pose + log.info("Moving to home pose"); robot.setMode(flexiv::Mode::NRT_PRIMITIVE_EXECUTION); - robot.executePrimitive("CaliForceSensor()"); + robot.executePrimitive("Home()"); + + // Wait for the primitive to finish + while (robot.isBusy()) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + + // Real-time Cartesian Motion Control + // ========================================================================================= + // 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"); + // Wait for primitive completion - log.warn("Calibrating force/torque sensors, please don't touch the robot"); while (robot.isBusy()) { std::this_thread::sleep_for(std::chrono::seconds(1)); } - log.info("Calibration complete"); + log.info("Sensor zeroing complete"); // Use robot base frame as reference frame for commands robot.setMode(flexiv::Mode::RT_CARTESIAN_MOTION_FORCE_BASE); @@ -243,13 +269,10 @@ int main(int argc, char* argv[]) // Set initial TCP pose robot.getRobotStates(robotStates); auto initTcpPose = robotStates.tcpPose; - log.info( - "Initial TCP pose set to [position 3x1, rotation (quaternion) " - "4x1]: " - + flexiv::utility::vec2Str(initTcpPose)); + log.info("Initial TCP pose set to [position 3x1, rotation (quaternion) 4x1]: " + + flexiv::utility::vec2Str(initTcpPose)); - // Periodic Tasks - //============================================================================= + // Create real-time scheduler to run periodic tasks flexiv::Scheduler scheduler; // Add periodic task with 1ms interval and highest applicable priority scheduler.addTask( diff --git a/example/RT_cartesian_motion_force_control.cpp b/example/intermediate5_realtime_cartesian_motion_force_control.cpp similarity index 68% rename from example/RT_cartesian_motion_force_control.cpp rename to example/intermediate5_realtime_cartesian_motion_force_control.cpp index 5e7f64f9..151d3c8f 100644 --- a/example/RT_cartesian_motion_force_control.cpp +++ b/example/intermediate5_realtime_cartesian_motion_force_control.cpp @@ -1,8 +1,8 @@ /** - * @example RT_cartesian_motion_force_control.cpp - * 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. + * @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. * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ @@ -34,6 +34,32 @@ constexpr double k_swingFreq = 0.3; constexpr double k_pressingForce = 5.0; } +/** @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::endl + << std::endl; +} + +/** @brief Print program usage help */ +void printHelp() +{ + // clang-format off + 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::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, @@ -60,8 +86,7 @@ void periodicTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv::Lo robotStates.extWrenchInBase[2]}; double extForceNorm = extForce.norm(); - // Set sign of Fz according to reference frame to achieve a "pressing - // down" behavior + // Set sign of Fz according to reference frame to achieve a "pressing down" behavior double Fz = 0.0; if (frameStr == "BASE") { Fz = -k_pressingForce; @@ -75,8 +100,8 @@ void periodicTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv::Lo // 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 + // 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 @@ -89,9 +114,8 @@ void periodicTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv::Lo return; } - // 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 + // 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) { @@ -101,9 +125,7 @@ void periodicTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv::Lo } } else if (loopCounter % (20 * k_loopFreq) == (15 * k_loopFreq)) { - log.info( - "Disabling force control and transiting smoothly to pure " - "motion control"); + 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 @@ -117,24 +139,22 @@ void periodicTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv::Lo = 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 + // 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 + // 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. + // 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); } @@ -147,39 +167,37 @@ void periodicTask(flexiv::Robot& robot, flexiv::Scheduler& scheduler, flexiv::Lo } } -void printHelp() -{ - // clang-format off - 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 << " --polish: execute a simple polish action along XY plane, " - "otherwise apply a constant force along Z axis" - << std::endl - << std::endl; - // clang-format on -} - int main(int argc, char* argv[]) { - // Log object for printing message with timestamp and coloring + // Program Setup + // ============================================================================================= + // Logger for printing message with timestamp and coloring flexiv::Log log; - // Parse Parameters - //============================================================================= + // Parse parameters if (argc < 3 || flexiv::utility::programArgsExistAny(argc, argv, {"-h", "--help"})) { printHelp(); return 1; } - // IP of the robot server std::string robotIP = argv[1]; - // IP of the workstation PC running this program std::string localIP = argv[2]; - // Whether to enable collision detection + // Print description + log.info("Tutorial description:"); + printDescription(); + + // The reference frame to use, see Robot::streamCartesianMotionForce() 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"); + frameStr = "TCP"; + } else { + log.info("Reference frame used for motion force control: robot base frame"); + } + + // Whether to enable polish action bool enablePolish = false; if (flexiv::utility::programArgsExist(argc, argv, "--polish")) { log.info("Robot will execute a polish action along XY plane"); @@ -190,7 +208,7 @@ int main(int argc, char* argv[]) try { // RDK Initialization - //============================================================================= + // ========================================================================================= // Instantiate robot interface flexiv::Robot robot(robotIP, localIP); @@ -221,62 +239,58 @@ int main(int argc, char* argv[]) 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"); + "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"); - // IMPORTANT: must calibrate force/torque sensor for accurate force - // control + // Move robot to home pose + log.info("Moving to home pose"); robot.setMode(flexiv::Mode::NRT_PRIMITIVE_EXECUTION); - robot.executePrimitive("CaliForceSensor()"); - // Wait for primitive completion - log.warn("Calibrating force/torque sensors, please don't touch the robot"); + robot.executePrimitive("Home()"); + + // Wait for the primitive to finish while (robot.isBusy()) { std::this_thread::sleep_for(std::chrono::seconds(1)); } - log.info("Calibration complete"); - - // Ask to specify reference frame for motion force control, see - // Robot::streamCartesianMotionForce() for more details - log.info("Please specify reference frame for motion force control:"); - std::cout << "[1] Base frame" << std::endl; - std::cout << "[2] TCP frame" << std::endl; - std::string frameStr = ""; - int userInput = 0; - std::cin >> userInput; - if (userInput == 1) { - frameStr = "BASE"; - robot.setMode(flexiv::Mode::RT_CARTESIAN_MOTION_FORCE_BASE); - } else if (userInput == 2) { - frameStr = "TCP"; - robot.setMode(flexiv::Mode::RT_CARTESIAN_MOTION_FORCE_TCP); - } else { - throw flexiv::InputException("Invalid reference frame choice"); + + // Real-time Cartesian Motion-force Control + // ========================================================================================= + // 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"); + + // Wait for primitive completion + while (robot.isBusy()) { + std::this_thread::sleep_for(std::chrono::seconds(1)); } - log.info("Using reference frame: " + frameStr); + log.info("Sensor zeroing complete"); - // Set initial TCP pose according to reference frame - std::vector initPose; + // Get latest robot states robot.getRobotStates(robotStates); + + // 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") { - // If using TCP frame, current TCP is at the reference frame's - // origin + 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"); } - log.info( - "Initial TCP pose set to [position 3x1, rotation (quaternion) " - "4x1]: " - + flexiv::utility::vec2Str(initPose)); + log.info("Initial TCP pose set to [position 3x1, rotation (quaternion) 4x1]: " + + flexiv::utility::vec2Str(initPose)); - // Periodic Tasks - //============================================================================= + // Create real-time scheduler to run periodic tasks flexiv::Scheduler scheduler; // Add periodic task with 1ms interval and highest applicable priority scheduler.addTask( diff --git a/example/robot_dynamics.cpp b/example/intermediate6_robot_dynamics.cpp similarity index 76% rename from example/robot_dynamics.cpp rename to example/intermediate6_robot_dynamics.cpp index f0e32c7c..d64e8dc9 100644 --- a/example/robot_dynamics.cpp +++ b/example/intermediate6_robot_dynamics.cpp @@ -1,7 +1,7 @@ /** - * @example robot_dynamics.cpp - * Run the integrated dynamics engine API flexiv::Model and print some results. - * Not running at 1kHz so that this example can also be executed on Mac. + * @example intermediate6_robot_dynamics.cpp + * This tutorial runs the integrated dynamics engine to obtain robot Jacobian, mass matrix, and + * gravity force. * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ @@ -18,10 +18,31 @@ #include #include -/** Periodic task running at around 100 Hz */ +/** @brief Print tutorial description */ +void printDescription() +{ + std::cout << "This tutorial runs the integrated dynamics engine to obtain robot Jacobian, mass " + "matrix, and gravity force." + << std::endl + << std::endl; +} + +/** @brief Print program usage help */ +void printHelp() +{ + // clang-format off + 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: None" << std::endl; + std::cout << std::endl; + // clang-format on +} + +/** @brief Periodic task running at 100 Hz */ int periodicTask(flexiv::Robot& robot, flexiv::Model& model) { - // Log object for printing message with timestamp and coloring + // Logger for printing message with timestamp and coloring flexiv::Log log; // Data struct for storing robot states @@ -68,14 +89,11 @@ int periodicTask(flexiv::Robot& robot, flexiv::Model& model) // 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; } @@ -86,38 +104,30 @@ int periodicTask(flexiv::Robot& robot, flexiv::Model& model) } } -void printHelp() -{ - // clang-format off - 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: None" << std::endl; - std::cout << std::endl; - // clang-format on -} - int main(int argc, char* argv[]) { - // Log object for printing message with timestamp and coloring + // Program Setup + // ============================================================================================= + // Logger for printing message with timestamp and coloring flexiv::Log log; - // Parse Parameters - //============================================================================= + // Parse parameters if (argc < 3 || flexiv::utility::programArgsExistAny(argc, argv, {"-h", "--help"})) { printHelp(); return 1; } - // IP of the robot server std::string robotIP = argv[1]; - // IP of the workstation PC running this program std::string localIP = argv[2]; + // Print description + log.info("Tutorial description:"); + printDescription(); + try { // RDK Initialization - //============================================================================= + // ========================================================================================= // Instantiate robot interface flexiv::Robot robot(robotIP, localIP); @@ -145,31 +155,27 @@ int main(int argc, char* argv[]) 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"); + "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"); - // Set mode after robot is operational - robot.setMode(flexiv::Mode::NRT_PLAN_EXECUTION); + // Move robot to home pose + log.info("Moving to home pose"); + robot.setMode(flexiv::Mode::NRT_PRIMITIVE_EXECUTION); + robot.executePrimitive("Home()"); - // Bring robot to home - robot.executePlan("PLAN-Home"); - - // Wait for the execution to finish - do { + // Wait for the primitive to finish + while (robot.isBusy()) { std::this_thread::sleep_for(std::chrono::seconds(1)); - } while (robot.isBusy()); + } + // Robot Dynamics + // ========================================================================================= // Initialize dynamics engine flexiv::Model model(robot); - // Periodic Tasks - //============================================================================= - // Use std::thread to do scheduling so that this example can run on both - // Linux and Mac, since the latter doesn't support flexiv::Scheduler std::thread periodicTaskThread(periodicTask, std::ref(robot), std::ref(model)); periodicTaskThread.join(); diff --git a/example/teach_by_demonstration.cpp b/example/intermediate7_teach_by_demonstration.cpp similarity index 81% rename from example/teach_by_demonstration.cpp rename to example/intermediate7_teach_by_demonstration.cpp index b6175b20..56c5aaad 100644 --- a/example/teach_by_demonstration.cpp +++ b/example/intermediate7_teach_by_demonstration.cpp @@ -1,7 +1,7 @@ /** - * @example teach_by_demonstration.cpp - * Free-drive the robot and record a series of Cartesian poses, which are then - * reproduced by the robot. + * @example intermediate7_teach_by_demonstration.cpp + * 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. * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ @@ -27,6 +27,17 @@ std::mutex g_userInputMutex; const std::vector k_maxContactWrench = {50.0, 50.0, 50.0, 15.0, 15.0, 15.0}; } +/** @brief Print tutorial description */ +void printDescription() +{ + std::cout + << "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." + << std::endl + << std::endl; +} + +/** @brief Print program usage help */ void printHelp() { // clang-format off @@ -40,25 +51,28 @@ void printHelp() int main(int argc, char* argv[]) { - // Log object for printing message with timestamp and coloring + // Program Setup + // ============================================================================================= + // Logger for printing message with timestamp and coloring flexiv::Log log; - // Parse Parameters - //============================================================================= + // Parse parameters if (argc < 3 || flexiv::utility::programArgsExistAny(argc, argv, {"-h", "--help"})) { printHelp(); return 1; } - // IP of the robot server std::string robotIP = argv[1]; - // IP of the workstation PC running this program std::string localIP = argv[2]; + // Print description + log.info("Tutorial description:"); + printDescription(); + try { // RDK Initialization - //============================================================================= + // ========================================================================================= // Instantiate robot interface flexiv::Robot robot(robotIP, localIP); @@ -86,15 +100,14 @@ int main(int argc, char* argv[]) 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"); + "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"); - // Application-specific Code - // ============================================================================= + // Teach By Demonstration + // ========================================================================================= // Recorded robot poses std::vector> savedPoses = {}; @@ -123,9 +136,7 @@ int main(int argc, char* argv[]) robot.executePlan("PLAN-FreeDriveAuto"); log.info("New teaching process started"); - log.warn( - "Hold down the enabling button on the motion bar to " - "activate free drive"); + log.warn("Hold down the enabling button on the motion bar to activate free drive"); } // Save current robot pose else if (inputBuffer == "r") { @@ -155,8 +166,7 @@ int main(int argc, char* argv[]) std::vector targetPos = {savedPoses[i][0], savedPoses[i][1], savedPoses[i][2]}; - // Convert quaternion to Euler ZYX required by - // MoveCompliance primitive + // Convert quaternion to Euler ZYX required by MoveCompliance primitive std::vector targetQuat = {savedPoses[i][3], savedPoses[i][4], savedPoses[i][5], savedPoses[i][6]}; auto targetEulerDeg @@ -177,9 +187,8 @@ int main(int argc, char* argv[]) } log.info( - "All saved poses are executed, enter 'n' to start a new " - "teaching process, 'r' to record more poses, 'e' to repeat " - "execution"); + "All saved poses are executed, enter 'n' to start a new teaching process, 'r' " + "to record more poses, 'e' to repeat execution"); // Put robot back to free drive robot.setMode(flexiv::Mode::NRT_PLAN_EXECUTION); diff --git a/example_py/display_robot_states.py b/example_py/basics1_display_robot_states.py similarity index 84% rename from example_py/display_robot_states.py rename to example_py/basics1_display_robot_states.py index cc2d3f0a..8b21826c 100644 --- a/example_py/display_robot_states.py +++ b/example_py/basics1_display_robot_states.py @@ -1,8 +1,9 @@ #!/usr/bin/env python -"""display_robot_states.py +"""basics1_display_robot_states.py -Print received robot states. +This tutorial does the very first thing: check connection with the robot server and print +received robot states. """ __copyright__ = "Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved." @@ -20,12 +21,21 @@ # fmt: on +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() + + def print_robot_states(robot, log): """ Print robot states data @ 1Hz. """ - # Data struct storing robot states robot_states = flexivrdk.RobotStates() @@ -57,21 +67,25 @@ def print_robot_states(robot, log): def main(): - # Parse Arguments - # ============================================================================= + # Program Setup + # ============================================================================================== + # 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') args = argparser.parse_args() # Define alias - # ============================================================================= log = flexivrdk.Log() mode = flexivrdk.Mode + # Print description + log.info("Tutorial description:") + print_description() + try: # RDK Initialization - # ============================================================================= + # ========================================================================================== # Instantiate robot interface robot = flexivrdk.Robot(args.robot_ip, args.local_ip) @@ -98,13 +112,12 @@ def main(): 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 booted " - "into Auto mode") + "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") - # Application-specific Code + # Print States # ============================================================================= # Thread for printing robot states print_thread = threading.Thread( diff --git a/example_py/clear_fault.py b/example_py/basics2_clear_fault.py similarity index 67% rename from example_py/clear_fault.py rename to example_py/basics2_clear_fault.py index c4c09385..8bc8fa86 100644 --- a/example_py/clear_fault.py +++ b/example_py/basics2_clear_fault.py @@ -1,8 +1,9 @@ #!/usr/bin/env python -"""clear_fault.py +"""basics2_clear_fault.py -Clear minor fault on the robot server side if any. +This tutorial clears minor faults from the robot server if any. Note that critical faults cannot +be cleared, see RDK manual for more details. """ __copyright__ = "Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved." @@ -19,27 +20,41 @@ # fmt: on +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() + + def main(): - # Parse Arguments - # ============================================================================= + # Program Setup + # ============================================================================================== + # 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') args = argparser.parse_args() # Define alias - # ============================================================================= log = flexivrdk.Log() + # Print description + log.info("Tutorial description:") + print_description() + try: # RDK Initialization - # ============================================================================= + # ========================================================================================== # Instantiate robot interface robot = flexivrdk.Robot(args.robot_ip, args.local_ip) - # Application-specific Code - # ============================================================================= - # Clear fault on robot server if any + # Fault Clearing + # ========================================================================================== + # Check if the robot has fault if robot.isFault(): log.warn("Fault occurred on robot server, trying to clear ...") # Try to clear the fault diff --git a/example_py/primitive_execution.py b/example_py/basics3_primitive_execution.py similarity index 68% rename from example_py/primitive_execution.py rename to example_py/basics3_primitive_execution.py index f5dbbf25..ee72b2d8 100644 --- a/example_py/primitive_execution.py +++ b/example_py/basics3_primitive_execution.py @@ -1,10 +1,9 @@ #!/usr/bin/env python -"""primitive_execution.py +"""basics3_primitive_execution.py -Execute various [Move] series primitives. -For detailed documentation on all available primitives, please see -[Flexiv Primitives](https://www.flexiv.com/primitives/). +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/). """ __copyright__ = "Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved." @@ -26,22 +25,37 @@ # fmt: on +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() + + def main(): - # Parse Arguments - # ============================================================================= + # Program Setup + # ============================================================================================== + # 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') args = argparser.parse_args() # Define alias - # ============================================================================= log = flexivrdk.Log() mode = flexivrdk.Mode + # Print description + log.info("Tutorial description:") + print_description() + try: # RDK Initialization - # ============================================================================= + # ========================================================================================== # Instantiate robot interface robot = flexivrdk.Robot(args.robot_ip, args.local_ip) @@ -68,22 +82,20 @@ def main(): 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 booted " - "into Auto mode") + "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") - # Set mode after robot is operational + # Execute Primitives + # ========================================================================================== + # Switch to primitive execution mode robot.setMode(mode.NRT_PRIMITIVE_EXECUTION) - # Application-specific Code - # ============================================================================= - # (1) Go to home pose - # ----------------------------------------------------------------------------- - # All parameters of the "Home" primitive are optional, - # thus we can skip the parameters and the default values will be used + # ------------------------------------------------------------------------------------------ + # All parameters of the "Home" primitive are optional, thus we can skip the parameters and + # the default values will be used log.info("Executing primitive: Home") # Send command to robot @@ -94,9 +106,8 @@ def main(): time.sleep(1) # (2) Move robot joints to target positions - # ----------------------------------------------------------------------------- - # The required parameter takes in 7 target joint positions - # Unit: degrees + # ------------------------------------------------------------------------------------------ + # The required parameter takes in 7 target joint positions. Unit: degrees log.info("Executing primitive: MoveJ") # Send command to robot @@ -107,43 +118,39 @@ def main(): time.sleep(1) # (3) Move robot TCP to a target position in world (base) frame - # ----------------------------------------------------------------------------- + # ------------------------------------------------------------------------------------------ # Required parameter: # target: final target position # [pos_x pos_y pos_z rot_x rot_y rot_z ref_frame ref_point] - # unit: m, deg + # Unit: m, deg # Optional parameter: # waypoints: waypoints to pass before reaching final target - # [same format as above, but can repeat for number of waypoints] + # (same format as above, but can repeat for number of waypoints) # maxVel: maximum TCP linear velocity - # unit: m/s - # NOTE: The rotations are using Euler ZYX convention, rot_x means Euler ZYX - # angle around X axis + # Unit: m/s + # NOTE: The rotations use Euler ZYX convention, rot_x means Euler ZYX angle around X axis log.info("Executing primitive: MoveL") # 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)") - - # 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. + "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)") + + # 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"): time.sleep(1) # (4) Another MoveL that uses TCP frame - # ---------------------------------------------------------------------------- - # In this example the reference frame is changed from - # WORLD::WORLD_ORIGIN to TRAJ::START, which represents the current TCP - # frame + # ------------------------------------------------------------------------------------------ + # In this example the reference frame is changed from WORLD::WORLD_ORIGIN to TRAJ::START, + # which represents the current TCP frame log.info("Executing primitive: MoveL") - # Example to convert target quaternion [w,x,y,z] to Euler ZYX using - # scipy package's 'xyz' extrinsic rotation + # Example to convert target quaternion [w,x,y,z] to Euler ZYX using scipy package's 'xyz' + # extrinsic rotation # NOTE: scipy uses [x,y,z,w] order to represent quaternion target_quat = [0.9185587, 0.1767767, 0.3061862, 0.1767767] # ZYX = [30, 30, 30] degrees diff --git a/example_py/plan_execution.py b/example_py/basics4_plan_execution.py similarity index 78% rename from example_py/plan_execution.py rename to example_py/basics4_plan_execution.py index f23402b2..e8491575 100644 --- a/example_py/plan_execution.py +++ b/example_py/basics4_plan_execution.py @@ -1,8 +1,11 @@ #!/usr/bin/env python -"""plan_execution.py +"""basics4_plan_execution.py -Select a plan from a list to execute using RDK's plan execution API. +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. """ __copyright__ = "Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved." @@ -19,23 +22,40 @@ # fmt: on +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() + + def main(): - # Parse Arguments - # ============================================================================= + # Program Setup + # ============================================================================================== + # 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') args = argparser.parse_args() # Define alias - # ============================================================================= log = flexivrdk.Log() mode = flexivrdk.Mode plan_info = flexivrdk.PlanInfo() + # Print description + log.info("Tutorial description:") + print_description() + try: # RDK Initialization - # ============================================================================= + # ========================================================================================== # Instantiate robot interface robot = flexivrdk.Robot(args.robot_ip, args.local_ip) @@ -62,17 +82,16 @@ def main(): 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 booted " - "into Auto mode") + "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") - # Set mode after robot is operational + # Execute Plans + # ========================================================================================== + # Switch to plan execution mode robot.setMode(mode.NRT_PLAN_EXECUTION) - # Application-specific Code - # ============================================================================= while True: # Monitor fault on robot server if robot.isFault(): diff --git a/example_py/basics5_zero_force_torque_sensors.py b/example_py/basics5_zero_force_torque_sensors.py new file mode 100644 index 00000000..26e6cb6e --- /dev/null +++ b/example_py/basics5_zero_force_torque_sensors.py @@ -0,0 +1,121 @@ +#!/usr/bin/env python + +"""basics5_zero_force_torque_sensors.py + +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. +""" + +__copyright__ = "Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved." +__author__ = "Flexiv" + +import time +import argparse +from utility import list2str + +# Import Flexiv RDK Python library +# fmt: off +import sys +sys.path.insert(0, "../lib_py") +import flexivrdk +# fmt: on + + +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() + + +def main(): + # Program Setup + # ============================================================================================== + # 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') + args = argparser.parse_args() + + # Define alias + log = flexivrdk.Log() + mode = flexivrdk.Mode + + # Print description + log.info("Tutorial description:") + print_description() + + try: + # RDK Initialization + # ========================================================================================== + # Instantiate robot interface + robot = flexivrdk.Robot(args.robot_ip, args.local_ip) + + # Clear fault on robot server if any + if robot.isFault(): + log.warn("Fault occurred on robot server, trying to clear ...") + # Try to clear the fault + robot.clearFault() + time.sleep(2) + # Check again + if robot.isFault(): + log.error("Fault cannot be cleared, exiting ...") + return + log.info("Fault on robot server is cleared") + + # Enable the robot, make sure the E-stop is released before enabling + log.info("Enabling robot ...") + 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") + + # Zero Sensors + # ========================================================================================== + # Get and print the current TCP force/moment readings + 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]") + + # Run the "ZeroFTSensor" primitive to automatically zero force and torque sensors + robot.setMode(mode.NRT_PRIMITIVE_EXECUTION) + 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") + + # Wait for the primitive completion + 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]") + + except Exception as e: + # Print exception error message + log.error(str(e)) + + +if __name__ == "__main__": + main() diff --git a/example_py/gripper_control.py b/example_py/basics6_gripper_control.py similarity index 78% rename from example_py/gripper_control.py rename to example_py/basics6_gripper_control.py index cc2ca39e..6c4bb1f5 100644 --- a/example_py/gripper_control.py +++ b/example_py/basics6_gripper_control.py @@ -1,9 +1,8 @@ #!/usr/bin/env python -"""clear_fault.py +"""basics6_gripper_control.py -Position and force control with grippers supported by Flexiv, while printing -gripper states at 1Hz. +This tutorial does position and force control (if available) for grippers supported by Flexiv. """ __copyright__ = "Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved." @@ -24,12 +23,21 @@ g_is_done = False +def print_description(): + """ + Print tutorial description. + + """ + print("This tutorial does position and force control (if available) for grippers " + "supported by Flexiv.") + print() + + def print_gripper_states(gripper, log): """ Print gripper states data @ 1Hz. """ - # Data struct storing gripper states gripper_states = flexivrdk.GripperStates() @@ -47,24 +55,25 @@ def print_gripper_states(gripper, log): def main(): - - # Parse Arguments - # ============================================================================= + # Program Setup + # ============================================================================================== + # 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 - # ============================================================================= log = flexivrdk.Log() mode = flexivrdk.Mode + # Print description + log.info("Tutorial description:") + print_description() + try: # RDK Initialization - # ============================================================================= + # ========================================================================================== # Instantiate robot interface robot = flexivrdk.Robot(args.robot_ip, args.local_ip) @@ -91,25 +100,20 @@ def main(): 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 booted " - "into Auto mode") + "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") - # Set mode after robot is operational + # Gripper Control + # ========================================================================================== + # Gripper control is not available if the robot is in IDLE mode, so switch to some mode + # other than IDLE robot.setMode(mode.NRT_PLAN_EXECUTION) - robot.executePlan("PLAN-Home") - # Wait for plan to start time.sleep(1) - # Wait for plan to finish - while robot.isBusy(): - time.sleep(1) - # Application-specific Code - # ============================================================================= - # Instantiate gripper + # Instantiate gripper control interface gripper = flexivrdk.Gripper(robot) # Thread for printing gripper states @@ -117,7 +121,7 @@ def main(): target=print_gripper_states, args=[gripper, log]) print_thread.start() - # Position control test + # Position control log.info("Closing gripper") gripper.move(0.01, 0.1, 20) time.sleep(2) @@ -125,7 +129,7 @@ def main(): gripper.move(0.09, 0.1, 20) time.sleep(2) - # Stop tests + # Stop log.info("Closing gripper") gripper.move(0.01, 0.1, 20) time.sleep(0.5) @@ -142,7 +146,7 @@ def main(): gripper.stop() time.sleep(2) - # Force control test, if available + # Force control, if available (sensed force is not zero) gripper_states = flexivrdk.GripperStates() gripper.getGripperStates(gripper_states) if abs(gripper_states.force) > sys.float_info.epsilon: diff --git a/example_py/auto_recovery.py b/example_py/basics7_auto_recovery.py similarity index 58% rename from example_py/auto_recovery.py rename to example_py/basics7_auto_recovery.py index 6ee120ef..a93a03ea 100644 --- a/example_py/auto_recovery.py +++ b/example_py/basics7_auto_recovery.py @@ -1,11 +1,9 @@ #!/usr/bin/env python -"""auto_recovery.py +"""basics7_auto_recovery.py -Run auto-recovery if the robot's safety system is in recovery state, -otherwise run damped floating with joint soft limit disabled, so that the -user can manually trigger a safety system recovery state by moving any joint -close to its limit. See flexiv::Robot::isRecoveryState() for more details. +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. """ __copyright__ = "Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved." @@ -21,23 +19,39 @@ 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() + def main(): - # Parse Arguments - # ============================================================================= + # Program Setup + # ============================================================================================== + # 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') args = argparser.parse_args() # Define alias - # ============================================================================= log = flexivrdk.Log() - mode = flexivrdk.Mode + + # Print description + log.info("Tutorial description:") + print_description() try: # RDK Initialization - # ============================================================================= + # ========================================================================================== # Instantiate robot interface robot = flexivrdk.Robot(args.robot_ip, args.local_ip) @@ -45,24 +59,23 @@ def main(): log.info("Enabling robot ...") robot.enable() - # Application-specific Code - # ============================================================================= - # If the system is in recovery state, we can't use isOperational to tell if - # the enabling process is done, so just wait long enough for the process to - # finish + # Run Auto-recovery + # ========================================================================================== + # If the system is in recovery state, we can't use isOperational to tell if the enabling + # 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 move back into allowed position range + # Start 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 auto - # recovery is done + # Block forever, must reboot the robot and restart user program after recovery is done while True: time.sleep(1) + # Otherwise the system is normal, do nothing else: - log.warn( + log.info( "Robot system is not in recovery state, nothing to be done, exiting ...") except Exception as e: diff --git a/example_py/NRT_joint_position_control.py b/example_py/intermediate1_non_realtime_joint_position_control.py similarity index 76% rename from example_py/NRT_joint_position_control.py rename to example_py/intermediate1_non_realtime_joint_position_control.py index a0be63fd..5df59b82 100644 --- a/example_py/NRT_joint_position_control.py +++ b/example_py/intermediate1_non_realtime_joint_position_control.py @@ -1,8 +1,8 @@ #!/usr/bin/env python -"""NRT_joint_position_control.py +"""intermediate1_non_realtime_joint_position_control.py -Non-real-time joint position control to hold or sine-sweep all joints. +This tutorial runs non-real-time joint position control to hold or sine-sweep all robot joints. """ __copyright__ = "Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved." @@ -20,15 +20,26 @@ # fmt: on +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() + + def main(): - # Parse Arguments - # ============================================================================= + # Program Setup + # ============================================================================================== + # Parse arguments argparser = argparse.ArgumentParser() # Required arguments 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, 1 to 200 [Hz]", type=int) + "frequency", help="command frequency, 20 to 200 [Hz]", type=int) # Optional arguments argparser.add_argument( "--hold", action="store_true", @@ -37,17 +48,20 @@ def main(): # Check if arguments are valid frequency = args.frequency - assert (frequency >= 1 and frequency <= 200), "Invalid input" + assert (frequency >= 20 and frequency <= 200), "Invalid input" # Define alias - # ============================================================================= robot_states = flexivrdk.RobotStates() log = flexivrdk.Log() mode = flexivrdk.Mode + # Print description + log.info("Tutorial description:") + print_description() + try: # RDK Initialization - # ============================================================================= + # ========================================================================================== # Instantiate robot interface robot = flexivrdk.Robot(args.robot_ip, args.local_ip) @@ -74,17 +88,25 @@ def main(): 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 booted " - "into Auto mode") + "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") - # Set mode after robot is operational + # Move robot to home pose + log.info("Moving to home pose") + robot.setMode(mode.NRT_PRIMITIVE_EXECUTION) + robot.executePrimitive("Home()") + + # Wait for the primitive to finish + while (robot.isBusy()): + time.sleep(1) + + # Non-real-time Joint Position Control + # ========================================================================================== + # Switch to non-real-time joint position control mode robot.setMode(mode.NRT_JOINT_POSITION) - # Application-specific Code - # ============================================================================= period = 1.0/frequency loop_time = 0 print("Sending command to robot at", frequency, diff --git a/example_py/NRT_cartesian_pure_motion_control.py b/example_py/intermediate2_non_realtime_cartesian_pure_motion_control.py similarity index 79% rename from example_py/NRT_cartesian_pure_motion_control.py rename to example_py/intermediate2_non_realtime_cartesian_pure_motion_control.py index 26631a6b..10e49f48 100644 --- a/example_py/NRT_cartesian_pure_motion_control.py +++ b/example_py/intermediate2_non_realtime_cartesian_pure_motion_control.py @@ -1,9 +1,9 @@ #!/usr/bin/env python -"""NRT_cartesian_pure_motion_control.py +"""intermediate2_non_realtime_cartesian_pure_motion_control.py -Non-real-time Cartesian-space pure motion control to hold or sine-sweep the robot TCP. -A simple collision detection is also included. +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. """ __copyright__ = "Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved." @@ -22,29 +22,40 @@ # fmt: on # Global constants -# ============================================================================= +# ================================================================================================== # TCP sine-sweep amplitude [m] SWING_AMP = 0.1 # TCP sine-sweep frequency [Hz] SWING_FREQ = 0.3 -# External TCP force threshold for collision detection [N] +# External TCP force threshold for collision detection, value is only for demo purpose [N] EXT_FORCE_THRESHOLD = 10.0 -# External joint torque threshold for collision detection [Nm] +# External joint torque threshold for collision detection, value is only for demo purpose [Nm] EXT_TORQUE_THRESHOLD = 5.0 +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() + + def main(): - # Parse Arguments - # ============================================================================= + # Program Setup + # ============================================================================================== + # Parse arguments argparser = argparse.ArgumentParser() # Required arguments 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, 1 to 200 [Hz]", type=int) + "frequency", help="command frequency, 20 to 200 [Hz]", type=int) # Optional arguments argparser.add_argument( "--hold", action="store_true", @@ -56,14 +67,17 @@ def main(): # Check if arguments are valid frequency = args.frequency - assert (frequency >= 1 and frequency <= 200), "Invalid input" + assert (frequency >= 20 and frequency <= 200), "Invalid input" # Define alias - # ============================================================================= robot_states = flexivrdk.RobotStates() log = flexivrdk.Log() mode = flexivrdk.Mode + # Print description + log.info("Tutorial description:") + print_description() + # Print based on arguments if args.hold: log.info("Robot holding current TCP pose") @@ -77,7 +91,7 @@ def main(): try: # RDK Initialization - # ============================================================================= + # ========================================================================================== # Instantiate robot interface robot = flexivrdk.Robot(args.robot_ip, args.local_ip) @@ -104,23 +118,34 @@ def main(): 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 booted " - "into Auto mode") + "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") - # Application-specific Code - # ============================================================================= - # IMPORTANT: must calibrate force/torque sensor for accurate collision - # detection + # Move robot to home pose + log.info("Moving to home pose") robot.setMode(mode.NRT_PRIMITIVE_EXECUTION) - robot.executePrimitive("CaliForceSensor()") + robot.executePrimitive("Home()") + + # Wait for the primitive to finish + while (robot.isBusy()): + time.sleep(1) + + # Non-real-time Cartesian Motion Control + # ========================================================================================== + # 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") + # Wait for primitive completion - log.warn("Calibrating force/torque sensors, please don't touch the robot") while robot.isBusy(): time.sleep(1) - log.info("Calibration complete") + log.info("Sensor zeroing complete") # Use robot base frame as reference frame for commands robot.setMode(mode.NRT_CARTESIAN_MOTION_FORCE_BASE) diff --git a/example_py/NRT_cartesian_motion_force_control.py b/example_py/intermediate3_non_realtime_cartesian_motion_force_control.py similarity index 77% rename from example_py/NRT_cartesian_motion_force_control.py rename to example_py/intermediate3_non_realtime_cartesian_motion_force_control.py index ffc13b57..242aab7d 100644 --- a/example_py/NRT_cartesian_motion_force_control.py +++ b/example_py/intermediate3_non_realtime_cartesian_motion_force_control.py @@ -1,10 +1,10 @@ #!/usr/bin/env python -"""NRT_cartesian_motion_force_control.py +"""intermediate3_non_realtime_cartesian_motion_force_control.py -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 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. """ __copyright__ = "Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved." @@ -23,7 +23,7 @@ # fmt: on # Global constants -# ============================================================================= +# ================================================================================================== # TCP sine-sweep amplitude [m] SWING_AMP = 0.1 @@ -34,16 +34,31 @@ PRESSING_FORCE = 5.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() + + def main(): - # Parse Arguments - # ============================================================================= + # Program Setup + # ============================================================================================== + # Parse arguments argparser = argparse.ArgumentParser() # Required arguments 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, 1 to 200 [Hz]", type=int) + "frequency", help="command frequency, 20 to 200 [Hz]", type=int) # Optional arguments + argparser.add_argument( + "--TCP", action="store_true", + help="use TCP frame as reference frame, 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") @@ -51,15 +66,26 @@ def main(): # Check if arguments are valid frequency = args.frequency - assert (frequency >= 1 and frequency <= 200), "Invalid input" + assert (frequency >= 20 and frequency <= 200), "Invalid input" # Define alias - # ============================================================================= robot_states = flexivrdk.RobotStates() log = flexivrdk.Log() mode = flexivrdk.Mode - # Print based on arguments + # Print description + log.info("Tutorial description:") + print_description() + + # 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") + frame_str = "TCP" + else: + log.info("Reference frame used for motion force control: robot base frame") + + # Whether to enable polish action if args.polish: log.info("Robot will execute a polish action along XY plane") else: @@ -67,7 +93,7 @@ def main(): try: # RDK Initialization - # ============================================================================= + # ========================================================================================== # Instantiate robot interface robot = flexivrdk.Robot(args.robot_ip, args.local_ip) @@ -94,51 +120,50 @@ def main(): 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 booted " - "into Auto mode") + "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") - # Application-specific Code - # ============================================================================= - # IMPORTANT: must calibrate force/torque sensor for accurate collision - # detection + # Move robot to home pose + log.info("Moving to home pose") robot.setMode(mode.NRT_PRIMITIVE_EXECUTION) - robot.executePrimitive("CaliForceSensor()") + robot.executePrimitive("Home()") + + # Wait for the primitive to finish + while (robot.isBusy()): + time.sleep(1) + + # Non-real-time Cartesian Motion-force Control + # ========================================================================================== + # 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") + # Wait for primitive completion - log.warn("Calibrating force/torque sensors, please don't touch the robot") while robot.isBusy(): time.sleep(1) - log.info("Calibration complete") - - # Ask to specify reference frame for motion force control, see - # Robot::streamCartesianMotionForce() for more details - log.info("Please specify reference frame for motion force control:") - print("[1] Base frame") - print("[2] TCP frame") - frame_str = "" - user_input = int(input()) - if user_input == 1: - frame_str = "BASE" - robot.setMode(mode.NRT_CARTESIAN_MOTION_FORCE_BASE) - elif user_input == 2: - frame_str = "TCP" - robot.setMode(mode.NRT_CARTESIAN_MOTION_FORCE_TCP) - else: - raise Exception("Invalid reference frame choice") + log.info("Sensor zeroing complete") - log.info("Using reference frame: " + frame_str) + # Get latest robot states + robot.getRobotStates(robot_states) - # Set initial TCP pose according to reference frame + # Set control mode and initial pose based on reference frame used init_pose = [] - robot.getRobotStates(robot_states) 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") print( "Initial TCP pose set to [position 3x1, rotation (quaternion) 4x1]: ", diff --git a/example_py/teach_by_demonstration.py b/example_py/intermediate4_teach_by_demonstration.py similarity index 85% rename from example_py/teach_by_demonstration.py rename to example_py/intermediate4_teach_by_demonstration.py index 3d4118cf..4fd100ad 100644 --- a/example_py/teach_by_demonstration.py +++ b/example_py/intermediate4_teach_by_demonstration.py @@ -1,9 +1,9 @@ #!/usr/bin/env python -"""teach_by_demonstration.py +"""intermediate4_teach_by_demonstration.py -Free-drive the robot and record a series of Cartesian poses, which are then -reproduced by the robot. +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. """ __copyright__ = "Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved." @@ -28,22 +28,36 @@ MAX_CONTACT_WRENCH = [50.0, 50.0, 50.0, 15.0, 15.0, 15.0] +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() + + def main(): - # Parse Arguments - # ============================================================================= + # Program Setup + # ============================================================================================== + # 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') args = argparser.parse_args() # Define alias - # ============================================================================= log = flexivrdk.Log() mode = flexivrdk.Mode + # Print description + log.info("Tutorial description:") + print_description() + try: # RDK Initialization - # ============================================================================= + # ========================================================================================== # Instantiate robot interface robot = flexivrdk.Robot(args.robot_ip, args.local_ip) @@ -70,14 +84,13 @@ def main(): 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 booted " - "into Auto mode") + "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") - # Application-specific Code - # ============================================================================= + # Teach By Demonstration + # ========================================================================================== # Recorded robot poses saved_poses = [] @@ -107,8 +120,7 @@ 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(): @@ -134,8 +146,7 @@ def main(): target_pos = [ saved_poses[i][0], saved_poses[i][1], saved_poses[i][2]] - # Convert quaternion to Euler ZYX required by - # MoveCompliance primitive + # 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]]