diff --git a/.github/workflows/cmake.yml b/.github/workflows/cmake.yml index ab5ee192..265b74f3 100644 --- a/.github/workflows/cmake.yml +++ b/.github/workflows/cmake.yml @@ -35,7 +35,7 @@ jobs: # Build examples and tests run: | cd ${{github.workspace}}/build - make -j4 + make -j$(nproc) build-ubuntu18: # The CMake configure and build commands are platform agnostic and should work equally @@ -60,6 +60,6 @@ jobs: # Build examples and tests run: | cd ${{github.workspace}}/build - make -j4 + make -j$(nproc) \ No newline at end of file diff --git a/README.md b/README.md index bda55174..b001eb79 100644 --- a/README.md +++ b/README.md @@ -3,7 +3,9 @@ ![CMake Badge](https://github.com/flexivrobotics/flexiv_rdk/actions/workflows/cmake.yml/badge.svg) [![License](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) -Flexiv Robot Development Kit (RDK) is a C++ library for creating powerful real-time applications with Flexiv robots. +Flexiv RDK (Robot Development Kit) is a powerful toolkit as an add-on to the Flexiv software platform. It enables the users to create complex applications with APIs that provide low-level real-time access to the robot. + +**C++** interface (real-time and non-real-time APIs) and **Python** interface (non-real-time APIs only) are supported. ## License @@ -11,13 +13,15 @@ Flexiv RDK is licensed under the [Apache 2.0 license](https://www.apache.org/lic ## References -Please refer to **[Flexiv RDK Manual](https://flexivrobotics.github.io/)** on how to properly set up the system and use Flexiv RDK library. +**[Flexiv RDK Manual](https://flexivrobotics.github.io/)** is the main reference and contains important information on how to properly set up the system and use Flexiv RDK library. + +[API documentation](https://flexivrobotics.github.io/flexiv_rdk/) contains details about available APIs, generated from Doxygen. -For details about the APIs, please refer to the [Doxygen generated API documentation](https://flexivrobotics.github.io/flexiv_rdk/). +## Run example programs -## Build and run the examples +**NOTE:** the instruction below is only a quick reference, assuming you've already gone through the Flexiv RDK Manual. -**NOTE:** the following is only a quick reference, assuming you've already gone through Flexiv RDK Manual. +### C++ interface 1. Configure and build example programs: @@ -27,14 +31,27 @@ For details about the APIs, please refer to the [Doxygen generated API documenta make -j4 2. The compiled program binaries will be output to ``flexiv_rdk/build/example`` -3. Assuming the robot is booted and connected, to run an example program: +3. Assume the robot is booted and connected, to run an example program: cd flexiv_rdk/build/example - sudo ./ + sudo ./ <...> + +### Python interface + +**NOTE:** Python 3.8 is required to use this interface, see Flexiv RDK Manual for more details. + +1. Make sure your Python version is 3.8.x: + + python3 --version + +2. Assume the robot is booted and connected, to run an example program: + + cd flexiv_rdk/example_py + sudo python3 .py <...> ## Integrated visualization -Flexiv RDK has integrated visualization based on Meshcat. To use it, follow instructions below. +Flexiv RDK has integrated visualization (only available for C++ interface) based on Meshcat. To use this feature: 1. Install Meshcat server using ``pip``: diff --git a/doc/Doxyfile.in b/doc/Doxyfile.in index b27dc015..ecb896b5 100755 --- a/doc/Doxyfile.in +++ b/doc/Doxyfile.in @@ -38,7 +38,7 @@ PROJECT_NAME = "RDK" # could be handy for archiving the generated documentation or if some version # control system is used. -PROJECT_NUMBER = "0.3.0" +PROJECT_NUMBER = "0.4.0" # Using the PROJECT_BRIEF tag one can provide an optional one line description # for a project that appears at the top of each page and should give viewer a diff --git a/example/auto_recovery.cpp b/example/auto_recovery.cpp index 4a3120cd..bb1aade3 100644 --- a/example/auto_recovery.cpp +++ b/example/auto_recovery.cpp @@ -4,7 +4,7 @@ * 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::Mode::MODE_AUTO_RECOVERY for more details. - * @copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. + * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ @@ -14,43 +14,14 @@ #include #include -namespace { -// robot DOF -const int k_robotDofs = 7; - -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 -void periodicTask(std::shared_ptr robotStates, - std::shared_ptr robot) -{ - // read robot states - robot->getRobotStates(robotStates.get()); - - // set 0 joint torques - std::vector torqueDesired(k_robotDofs, 0.0); - - // add some velocity damping - for (size_t i = 0; i < k_robotDofs; ++i) { - torqueDesired[i] = -k_floatingDamping[i] * robotStates->m_dtheta[i]; - } - - // send target joint torque to RDK server, enable gravity compensation - // and joint-space APF - robot->streamJointTorque(torqueDesired, true, false); -} - int main(int argc, char* argv[]) { - // log object for printing message with timestamp and coloring + // Log object for printing message with timestamp and coloring flexiv::Log log; // Parse Parameters //============================================================================= - // check if program has 3 arguments + // Check if program has 3 arguments if (argc != 3) { log.error("Invalid program arguments. Usage: "); return 0; @@ -63,55 +34,48 @@ int main(int argc, char* argv[]) // RDK Initialization //============================================================================= - // instantiate robot interface + // Instantiate robot interface auto robot = std::make_shared(); - // create data struct for storing robot states + // Create data struct for storing robot states auto robotStates = std::make_shared(); - // initialize robot interface and connect to the robot server + // Initialize robot interface and connect to the robot server robot->init(robotIP, localIP); - // wait for the connection to be established + // Wait for the connection to be established do { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } while (!robot->isConnected()); - // enable the robot, make sure the E-stop is released before enabling + // Enable the robot, make sure the E-stop is released before enabling if (robot->enable()) { log.info("Enabling robot ..."); } - // if the system is in recovery state, we can't use isOperational to tell if - // the enable process is done, so just wait for long enough + // 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 std::this_thread::sleep_for(std::chrono::seconds(8)); - // Auto recover if needed - //============================================================================= - // 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 robot and restart user program after auto - // recovery is done + // Block forever, must reboot the robot and restart user program after + // auto recovery is done do { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } while (true); } - // otherwise the system is normal, run robot floating using joint torques + // Otherwise the system is normal, do nothing else { - robot->setMode(flexiv::MODE_JOINT_TORQUE); - // wait for the mode to be switched - do { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } while (robot->getMode() != flexiv::MODE_JOINT_TORQUE); + log.info( + "Robot system is not in recovery state, nothing to be done, " + "exiting ..."); } - // High-priority Realtime Periodic Task @ 1kHz - //============================================================================= - // this is a blocking method, so all other user-defined background - // threads should be spawned before this - robot->start(std::bind(periodicTask, robotStates, robot)); - return 0; } diff --git a/example/cartesian_impedance_control.cpp b/example/cartesian_impedance_control.cpp index 904ee079..4db41944 100644 --- a/example/cartesian_impedance_control.cpp +++ b/example/cartesian_impedance_control.cpp @@ -1,7 +1,7 @@ /** * @example cartesian_impedance_control.cpp * Run Cartesian impedance control to hold or sine-sweep the robot TCP. - * @copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. + * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ @@ -14,31 +14,32 @@ namespace { -// size of Cartesian pose vector [position 3x1 + rotation (quaternion) 4x1 ] +/** Size of Cartesian pose vector [position 3x1 + rotation (quaternion) 4x1 ] */ const unsigned int k_cartPoseSize = 7; -// RT loop period [sec] +/** RT loop period [sec] */ const double k_loopPeiord = 0.001; -// TCP sine-sweep amplitude [m] +/** TCP sine-sweep amplitude [m] */ const double k_swingAmp = 0.1; -// TCP sine-sweep frequency [Hz] +/** TCP sine-sweep frequency [Hz] */ const double k_swingFreq = 0.3; -// initial Cartesian-space pose (position + rotation) of robot TCP +/** Initial Cartesian-space pose (position + rotation) of robot TCP */ std::vector g_initTcpPose; -// current Cartesian-space pose (position + rotation) of robot TCP +/** Current Cartesian-space pose (position + rotation) of robot TCP */ std::vector g_currentTcpPose; -// flag whether initial Cartesian position is set +/** Flag whether initial Cartesian position is set */ bool g_isInitPoseSet = false; -// loop counter +/** Loop counter */ unsigned int g_loopCounter = 0; } +/** Helper function to print a std::vector */ void printVector(const std::vector& vec) { for (auto i : vec) { @@ -47,20 +48,20 @@ void printVector(const std::vector& vec) std::cout << std::endl; } -// callback function for realtime periodic task +/** Callback function for realtime periodic task */ void periodicTask(std::shared_ptr robotStates, std::shared_ptr robot, std::string motionType) { - // read robot states + // Read robot states robot->getRobotStates(robotStates.get()); - // use target TCP velocity and acceleration = 0 + // Use target TCP velocity and acceleration = 0 std::vector tcpVel(6, 0); std::vector tcpAcc(6, 0); - // set initial TCP pose + // Set initial TCP pose if (!g_isInitPoseSet) { - // check vector size before saving + // Check vector size before saving if (robotStates->m_tcpPose.size() == k_cartPoseSize) { g_initTcpPose = robotStates->m_tcpPose; g_currentTcpPose = g_initTcpPose; @@ -70,9 +71,9 @@ void periodicTask(std::shared_ptr robotStates, printVector(g_initTcpPose); } } - // run control only after initial pose is set + // Run control only after initial pose is set else { - // set target position based on motion type + // Set target position based on motion type if (motionType == "hold") { robot->streamTcpPose(g_initTcpPose, tcpVel, tcpAcc); } else if (motionType == "sine-sweep") { @@ -93,12 +94,12 @@ void periodicTask(std::shared_ptr robotStates, int main(int argc, char* argv[]) { - // log object for printing message with timestamp and coloring + // Log object for printing message with timestamp and coloring flexiv::Log log; // Parse Parameters //============================================================================= - // check if program has 3 arguments + // Check if program has 3 arguments if (argc != 4) { log.error( "Invalid program arguments. Usage: " @@ -112,51 +113,51 @@ int main(int argc, char* argv[]) // IP of the workstation PC running this program std::string localIP = argv[2]; - // type of motion specified by user + // Type of motion specified by user std::string motionType = argv[3]; // RDK Initialization //============================================================================= - // instantiate robot interface + // Instantiate robot interface auto robot = std::make_shared(); - // create data struct for storing robot states + // Create data struct for storing robot states auto robotStates = std::make_shared(); - // initialize robot interface and connect to the robot server + // Initialize robot interface and connect to the robot server robot->init(robotIP, localIP); - // wait for the connection to be established + // Wait for the connection to be established do { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } while (!robot->isConnected()); - // enable the robot, make sure the E-stop is released before enabling + // Enable the robot, make sure the E-stop is released before enabling if (robot->enable()) { log.info("Enabling robot ..."); } - // wait for the robot to become operational + // Wait for the robot to become operational do { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } while (!robot->isOperational()); log.info("Robot is now operational"); - // set mode after robot is operational + // Set mode after robot is operational robot->setMode(flexiv::MODE_CARTESIAN_IMPEDANCE); - // wait for the mode to be switched + // Wait for the mode to be switched do { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } while (robot->getMode() != flexiv::MODE_CARTESIAN_IMPEDANCE); - // choose the index of tool being used. No need to call this method if the + // Choose the index of tool being used. No need to call this method if the // mounted tool on the robot has only one TCP, it'll be used by default robot->switchTcp(0); // High-priority Realtime Periodic Task @ 1kHz //============================================================================= - // this is a blocking method, so all other user-defined background threads + // This is a blocking method, so all other user-defined background threads // should be spawned before this robot->start(std::bind(periodicTask, robotStates, robot, motionType)); diff --git a/example/display_robot_states.cpp b/example/display_robot_states.cpp index d267128a..e5e790c4 100644 --- a/example/display_robot_states.cpp +++ b/example/display_robot_states.cpp @@ -1,7 +1,7 @@ /** * @example display_robot_states.cpp * Print received robot states. - * @copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. + * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ @@ -14,45 +14,43 @@ #include namespace { - -// print counter +/** Print counter */ unsigned int g_printCounter = 0; -// robot states to be printed in another low-priority thread +/** Robot states to be printed in another low-priority thread */ flexiv::RobotStates g_robotStatesPrintBuffer; -// mutex on robot states print buffer +/** Mutex on robot states print buffer */ std::mutex g_printBufferMutex; - } -// user-defined high-priority realtime periodic task, running at 1kHz +/** User-defined high-priority realtime periodic task, running at 1kHz */ void highPriorityPeriodicTask(std::shared_ptr robotStates, std::shared_ptr robot) { - // get robot states + // Get robot states robot->getRobotStates(robotStates.get()); - // save data to buffer for printing in another thread + // Save data to buffer for printing in another thread { std::lock_guard lock(g_printBufferMutex); g_robotStatesPrintBuffer = *(robotStates.get()); } } -// user-defined low-priority non-realtime task +// User-defined low-priority non-realtime task // NOT strictly scheduled at a fixed rate void lowPriorityTask() { - // wait a while for the data to start streaming + // Wait a while for the data to start streaming std::this_thread::sleep_for(std::chrono::seconds(2)); - // use while loop to prevent this thread from return + // Use while loop to prevent this thread from return while (true) { - // wake up every second to do something + // Wake up every second to do something std::this_thread::sleep_for(std::chrono::seconds(1)); - // safely save data in print buffer to local first to avoid prolonged + // Safely save data in print buffer to local first to avoid prolonged // mutex lock flexiv::RobotStates robotStates; { @@ -60,12 +58,12 @@ void lowPriorityTask() robotStates = g_robotStatesPrintBuffer; } - // divider + // Print divider std::cout << "\n\n[" << ++g_printCounter << "]"; std::cout << "=========================================================" << std::endl; - // print all robot states in JSON format using the built-in ostream + // Print all robot states in JSON format using the built-in ostream // operator overloading std::cout << robotStates << std::endl; } @@ -73,12 +71,12 @@ void lowPriorityTask() int main(int argc, char* argv[]) { - // log object for printing message with timestamp and coloring + // Log object for printing message with timestamp and coloring flexiv::Log log; // Parse Parameters //============================================================================= - // check if program has 3 arguments + // Check if program has 3 arguments if (argc != 3) { log.error("Invalid program arguments. Usage: "); return 0; @@ -91,42 +89,42 @@ int main(int argc, char* argv[]) // RDK Initialization //============================================================================= - // instantiate robot interface + // Instantiate robot interface auto robot = std::make_shared(); - // create data struct for storing robot states + // Create data struct for storing robot states auto robotStates = std::make_shared(); - // initialize robot interface and connect to the robot server + // Initialize robot interface and connect to the robot server robot->init(robotIP, localIP); - // wait for the connection to be established + // Wait for the connection to be established do { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } while (!robot->isConnected()); - // enable the robot, make sure the E-stop is released before enabling + // Enable the robot, make sure the E-stop is released before enabling if (robot->enable()) { log.info("Enabling robot ..."); } - // wait for the robot to become operational + // Wait for the robot to become operational do { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } while (!robot->isOperational()); log.info("Robot is now operational"); - // set mode after robot is operational + // Set mode after robot is operational robot->setMode(flexiv::MODE_IDLE); // Low-priority Background Tasks //============================================================================= - // use std::thread for some non-realtime tasks, not joining (non-blocking) + // Use std::thread for some non-realtime tasks, not joining (non-blocking) std::thread lowPriorityThread(lowPriorityTask); // High-priority Realtime Periodic Task @ 1kHz //============================================================================= - // this is a blocking method, so all other user-defined background threads + // This is a blocking method, so all other user-defined background threads // should be spawned before this robot->start(std::bind(highPriorityPeriodicTask, robotStates, robot)); diff --git a/example/floating_with_soft_limits.cpp b/example/floating_with_soft_limits.cpp index 5728e995..9b008bd2 100644 --- a/example/floating_with_soft_limits.cpp +++ b/example/floating_with_soft_limits.cpp @@ -2,7 +2,7 @@ * @example floating_with_soft_limits.cpp * Run damped floating with soft limits enabled to keep joints from * hitting position limits. - * @copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. + * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ @@ -13,41 +13,42 @@ #include namespace { -// robot DOF +/** Robot DOF */ const int k_robotDofs = 7; +/** 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 +/** Callback function for realtime periodic task */ void periodicTask(std::shared_ptr robotStates, std::shared_ptr robot) { - // read robot states + // Read robot states robot->getRobotStates(robotStates.get()); - // set 0 joint torques + // Set 0 joint torques std::vector torqueDesired(k_robotDofs, 0.0); - // add some velocity damping + // Add some velocity damping for (size_t i = 0; i < k_robotDofs; ++i) { torqueDesired[i] = -k_floatingDamping[i] * robotStates->m_dtheta[i]; } - // send target joint torque to RDK server, enable gravity compensation and + // Send target joint torque to RDK server, enable gravity compensation and // joint soft limits robot->streamJointTorque(torqueDesired, true, true); } int main(int argc, char* argv[]) { - // log object for printing message with timestamp and coloring + // Log object for printing message with timestamp and coloring flexiv::Log log; // Parse Parameters //============================================================================= - // check if program has 3 arguments + // Check if program has 3 arguments if (argc != 3) { log.error("Invalid program arguments. Usage: "); return 0; @@ -60,42 +61,42 @@ int main(int argc, char* argv[]) // RDK Initialization //============================================================================= - // instantiate robot interface + // Instantiate robot interface auto robot = std::make_shared(); - // create data struct for storing robot states + // Create data struct for storing robot states auto robotStates = std::make_shared(); - // initialize robot interface and connect to the robot server + // Initialize robot interface and connect to the robot server robot->init(robotIP, localIP); - // wait for the connection to be established + // Wait for the connection to be established do { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } while (!robot->isConnected()); - // enable the robot, make sure the E-stop is released before enabling + // Enable the robot, make sure the E-stop is released before enabling if (robot->enable()) { log.info("Enabling robot ..."); } - // wait for the robot to become operational + // Wait for the robot to become operational do { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } while (!robot->isOperational()); log.info("Robot is now operational"); - // set mode after robot is operational + // Set mode after robot is operational robot->setMode(flexiv::MODE_JOINT_TORQUE); - // wait for the mode to be switched + // Wait for the mode to be switched do { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } while (robot->getMode() != flexiv::MODE_JOINT_TORQUE); // High-priority Realtime Periodic Task @ 1kHz //============================================================================= - // this is a blocking method, so all other user-defined background threads + // This is a blocking method, so all other user-defined background threads // should be spawned before this robot->start(std::bind(periodicTask, robotStates, robot)); diff --git a/example/joint_impedance_control.cpp b/example/joint_impedance_control.cpp index 7cad99da..e7c18f9f 100644 --- a/example/joint_impedance_control.cpp +++ b/example/joint_impedance_control.cpp @@ -1,7 +1,7 @@ /** * @example joint_impedance_control.cpp * Run joint impedance control using RDK's joint torque streaming API. - * @copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. + * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ @@ -14,33 +14,34 @@ #include namespace { -// robot DOF +/** Robot DOF */ const int k_robotDofs = 7; -// RT loop period [sec] +/** RT loop period [sec] */ const double k_loopPeiord = 0.001; -// joint impedance control gains +/** Joint impedance control gains */ const std::vector k_impedanceKp = {3000.0, 3000.0, 800.0, 800.0, 200.0, 200.0, 200.0}; const std::vector k_impedanceKd = {80.0, 80.0, 40.0, 40.0, 8.0, 8.0, 8.0}; -// sine-sweep trajectory amplitude and frequency +/** Sine-sweep trajectory amplitude and frequency */ const std::vector k_sineAmp = {0.035, 0.035, 0.035, 0.035, 0.035, 0.035, 0.035}; const std::vector k_sineFreq = {0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3}; -// initial position of robot joints +/** Initial position of robot joints */ std::vector g_initPosition; -// flag whether initial joint position is set +/** Flag whether initial joint position is set */ bool g_isInitPositionSet = false; -// loop counter +/** Loop counter */ unsigned int g_loopCounter = 0; } +/** Helper function to print a std::vector */ void printVector(const std::vector& vec) { for (auto i : vec) { @@ -49,17 +50,17 @@ void printVector(const std::vector& vec) std::cout << std::endl; } -// callback function for realtime periodic task +/** Callback function for realtime periodic task */ void periodicTask(std::shared_ptr robotStates, std::shared_ptr robot, std::string motionType, flexiv::Log& log) { - // read robot states + // Read robot states robot->getRobotStates(robotStates.get()); - // set initial joint position + // Set initial joint position if (!g_isInitPositionSet) { - // check vector size before saving + // Check vector size before saving if (robotStates->m_q.size() == k_robotDofs) { g_initPosition = robotStates->m_q; g_isInitPositionSet = true; @@ -67,14 +68,14 @@ void periodicTask(std::shared_ptr robotStates, printVector(g_initPosition); } } - // run control only after init position is set + // Run control only after init position is set else { - // initialize target position to hold position + // Initialize target position to hold position auto targetPosition = g_initPosition; - // set target position based on motion type + // Set target position based on motion type if (motionType == "hold") { - // do nothing, target position is initialized to hold position + // Do nothing, target position is initialized to hold position } else if (motionType == "sine-sweep") { for (size_t i = 0; i < k_robotDofs; ++i) { targetPosition[i] = g_initPosition[i] @@ -89,14 +90,14 @@ void periodicTask(std::shared_ptr robotStates, } std::vector torqueDesired(k_robotDofs); - // impedance control on all joints + // Impedance control on all joints for (size_t i = 0; i < k_robotDofs; ++i) { torqueDesired[i] = k_impedanceKp[i] * (targetPosition[i] - robotStates->m_q[i]) - k_impedanceKd[i] * robotStates->m_dtheta[i]; } - // send target joint torque to RDK server + // Send target joint torque to RDK server robot->streamJointTorque(torqueDesired, true); } @@ -105,12 +106,12 @@ void periodicTask(std::shared_ptr robotStates, int main(int argc, char* argv[]) { - // log object for printing message with timestamp and coloring + // Log object for printing message with timestamp and coloring flexiv::Log log; // Parse Parameters //============================================================================= - // check if program has 3 arguments + // Check if program has 3 arguments if (argc != 4) { log.error( "Invalid program arguments. Usage: " @@ -124,47 +125,47 @@ int main(int argc, char* argv[]) // IP of the workstation PC running this program std::string localIP = argv[2]; - // type of motion specified by user + // Type of motion specified by user std::string motionType = argv[3]; // RDK Initialization //============================================================================= - // instantiate robot interface + // Instantiate robot interface auto robot = std::make_shared(); - // create data struct for storing robot states + // Create data struct for storing robot states auto robotStates = std::make_shared(); - // initialize robot interface and connect to the robot server + // Initialize robot interface and connect to the robot server robot->init(robotIP, localIP); - // wait for the connection to be established + // Wait for the connection to be established do { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } while (!robot->isConnected()); - // enable the robot, make sure the E-stop is released before enabling + // Enable the robot, make sure the E-stop is released before enabling if (robot->enable()) { log.info("Enabling robot ..."); } - // wait for the robot to become operational + // Wait for the robot to become operational do { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } while (!robot->isOperational()); log.info("Robot is now operational"); - // set mode after robot is operational + // Set mode after robot is operational robot->setMode(flexiv::MODE_JOINT_TORQUE); - // wait for the mode to be switched + // Wait for the mode to be switched do { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } while (robot->getMode() != flexiv::MODE_JOINT_TORQUE); // High-priority Realtime Periodic Task @ 1kHz //============================================================================= - // this is a blocking method, so all other user-defined background threads + // This is a blocking method, so all other user-defined background threads // should be spawned before this robot->start(std::bind(periodicTask, robotStates, robot, motionType, log)); diff --git a/example/joint_position_control.cpp b/example/joint_position_control.cpp index cb002cb6..f2a900d2 100644 --- a/example/joint_position_control.cpp +++ b/example/joint_position_control.cpp @@ -1,7 +1,7 @@ /** * @example joint_position_control.cpp - * Run joint position control using RDK's joint position streaming API. - * @copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. + * Run joint position control to hold or sine-sweep all joints. + * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ @@ -14,27 +14,28 @@ #include namespace { -// robot DOF +/** Robot DOF */ const int k_robotDofs = 7; -// RT loop period [sec] +/** RT loop period [sec] */ const double k_loopPeiord = 0.001; -// sine-sweep trajectory amplitude and frequency +/** Sine-sweep trajectory amplitude and frequency */ const std::vector k_sineAmp = {0.035, 0.035, 0.035, 0.035, 0.035, 0.035, 0.035}; const std::vector k_sineFreq = {0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3}; -// initial position of robot joints +/** Initial position of robot joints */ std::vector g_initPosition; -// flag whether initial joint position is set +/** Flag whether initial joint position is set */ bool g_isInitPositionSet = false; -// loop counter +/** Loop counter */ unsigned int g_loopCounter = 0; } +/** Helper function to print a std::vector */ void printVector(const std::vector& vec) { for (auto i : vec) { @@ -43,17 +44,17 @@ void printVector(const std::vector& vec) std::cout << std::endl; } -// callback function for realtime periodic task +/** Callback function for realtime periodic task */ void periodicTask(std::shared_ptr robotStates, std::shared_ptr robot, std::string motionType, flexiv::Log& log) { - // read robot states + // Read robot states robot->getRobotStates(robotStates.get()); - // set initial joint position + // Set initial joint position if (!g_isInitPositionSet) { - // check vector size before saving + // Check vector size before saving if (robotStates->m_q.size() == k_robotDofs) { g_initPosition = robotStates->m_q; g_isInitPositionSet = true; @@ -61,16 +62,16 @@ void periodicTask(std::shared_ptr robotStates, printVector(g_initPosition); } } - // run control only after init position is set + // Run control only after init position is set else { - // initialize target vectors to hold position + // Initialize target vectors to hold position auto targetPosition = g_initPosition; std::vector targetVelocity(k_robotDofs, 0); std::vector targetAcceleration(k_robotDofs, 0); - // set target vectors based on motion type + // Set target vectors based on motion type if (motionType == "hold") { - // do nothing, target vectors are initialized to hold position + // Do nothing, target vectors are initialized to hold position } else if (motionType == "sine-sweep") { for (size_t i = 0; i < k_robotDofs; ++i) { targetPosition[i] = g_initPosition[i] @@ -87,7 +88,7 @@ void periodicTask(std::shared_ptr robotStates, exit(1); } - // send target joint position to RDK server + // Send target joint position to RDK server robot->streamJointPosition( targetPosition, targetVelocity, targetAcceleration); } @@ -97,12 +98,12 @@ void periodicTask(std::shared_ptr robotStates, int main(int argc, char* argv[]) { - // log object for printing message with timestamp and coloring + // Log object for printing message with timestamp and coloring flexiv::Log log; // Parse Parameters //============================================================================= - // check if program has 3 arguments + // Check if program has 3 arguments if (argc != 4) { log.error( "Invalid program arguments. Usage: " @@ -116,47 +117,47 @@ int main(int argc, char* argv[]) // IP of the workstation PC running this program std::string localIP = argv[2]; - // type of motion specified by user + // Type of motion specified by user std::string motionType = argv[3]; // RDK Initialization //============================================================================= - // instantiate robot interface + // Instantiate robot interface auto robot = std::make_shared(); - // create data struct for storing robot states + // Create data struct for storing robot states auto robotStates = std::make_shared(); - // initialize robot interface and connect to the robot server + // Initialize robot interface and connect to the robot server robot->init(robotIP, localIP); - // wait for the connection to be established + // Wait for the connection to be established do { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } while (!robot->isConnected()); - // enable the robot, make sure the E-stop is released before enabling + // Enable the robot, make sure the E-stop is released before enabling if (robot->enable()) { log.info("Enabling robot ..."); } - // wait for the robot to become operational + // Wait for the robot to become operational do { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } while (!robot->isOperational()); log.info("Robot is now operational"); - // set mode after robot is operational + // Set mode after robot is operational robot->setMode(flexiv::MODE_JOINT_POSITION); - // wait for the mode to be switched + // Wait for the mode to be switched do { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } while (robot->getMode() != flexiv::MODE_JOINT_POSITION); // High-priority Realtime Periodic Task @ 1kHz //============================================================================= - // this is a blocking method, so all other user-defined background threads + // This is a blocking method, so all other user-defined background threads // should be spawned before this robot->start(std::bind(periodicTask, robotStates, robot, motionType, log)); diff --git a/example/plan_execution.cpp b/example/plan_execution.cpp index 7518dbe2..267682c1 100644 --- a/example/plan_execution.cpp +++ b/example/plan_execution.cpp @@ -1,7 +1,7 @@ /** * @example plan_execution.cpp * Select a plan from a list to execute using RDK's plan execution API. - * @copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. + * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ @@ -15,25 +15,25 @@ #include namespace { -// user input string +/** User input string */ std::string g_userInput; -// user input mutex +/** User input mutex */ std::mutex g_userInputMutex; } -// callback function for user input state machine +/** Callback function for user input state machine */ void userInputStateMachine(std::shared_ptr robot) { - // log object for printing message with timestamp and coloring + // Log object for printing message with timestamp and coloring flexiv::Log log; - // use while loop to prevent this thread from return + // Use while loop to prevent this thread from return while (true) { - // wake up every 0.1 second to do something + // Wake up every 0.1 second to do something std::this_thread::sleep_for(std::chrono::milliseconds(100)); - // check user input + // Check user input std::string inputBuffer; { std::lock_guard lock(g_userInputMutex); @@ -41,52 +41,51 @@ void userInputStateMachine(std::shared_ptr robot) } if (inputBuffer.empty()) { - // do nothing, waiting for new user input or plan to finish + // Do nothing, waiting for new user input or plan to finish } - // list available plans + // List available plans else if (inputBuffer == "l") { - std::vector planList; - robot->getPlanNameList(&planList); + auto planList = robot->getPlanNameList(); std::cout << "===================== Plan name list =====================" << std::endl; - // print plan list + // Print plan list for (unsigned int i = 0; i < planList.size(); i++) { std::cout << "[" << i << "] " << planList[i] << std::endl; } log.info("Enter index to select a plan to execute:"); } - // stop plan execution + // Stop plan execution else if (inputBuffer == "s") { robot->stop(); log.info("Execution stopped, enter index to execute another plan:"); - // after calling stop(), the robot will enter Idle mode, so put the + // After calling stop(), the robot will enter Idle mode, so put the // robot back to plan execution mode to take more plan commands // set mode after robot is operational robot->setMode(flexiv::MODE_PLAN_EXECUTION); - // wait for the mode to be switched + // Wait for the mode to be switched do { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } while (robot->getMode() != flexiv::MODE_PLAN_EXECUTION); } - // choose plan to execute + // Choose plan to execute else { - // check system status + // Check system status flexiv::SystemStatus systemStatus; robot->getSystemStatus(&systemStatus); - // execute new plan if no plan is running + // Execute new plan if no plan is running if (systemStatus.m_programRunning == false) { - // convert string to int + // Convert string to int int index = std::stoi(inputBuffer); - // start executing + // Start executing robot->executePlanByIndex(index); } } - // reset user input every loop + // Reset user input every loop { std::lock_guard lock(g_userInputMutex); g_userInput.clear(); @@ -96,12 +95,12 @@ void userInputStateMachine(std::shared_ptr robot) int main(int argc, char* argv[]) { - // log object for printing message with timestamp and coloring + // Log object for printing message with timestamp and coloring flexiv::Log log; // Parse Parameters //============================================================================= - // check if program has 3 arguments + // Check if program has 3 arguments if (argc != 3) { log.error("Invalid program arguments. Usage: "); return 0; @@ -117,39 +116,39 @@ int main(int argc, char* argv[]) // RDK robot client auto robot = std::make_shared(); - // create data struct for storing robot states + // Create data struct for storing robot states auto robotStates = std::make_shared(); - // initialize robot interface and connect to the robot server + // Initialize robot interface and connect to the robot server robot->init(robotIP, localIP); - // wait for the connection to be established + // Wait for the connection to be established do { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } while (!robot->isConnected()); - // enable the robot, make sure the E-stop is released before enabling + // Enable the robot, make sure the E-stop is released before enabling if (robot->enable()) { log.info("Enabling robot ..."); } - // wait for the robot to become operational + // Wait for the robot to become operational do { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } while (!robot->isOperational()); log.info("Robot is now operational"); - // set mode after robot is operational + // Set mode after robot is operational robot->setMode(flexiv::MODE_PLAN_EXECUTION); - // wait for the mode to be switched + // Wait for the mode to be switched do { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } while (robot->getMode() != flexiv::MODE_PLAN_EXECUTION); // Low-priority Background Tasks //============================================================================= - // use std::thread for some non-realtime tasks, not joining (non-blocking) + // Use std::thread for some non-realtime tasks, not joining (non-blocking) std::thread lowPriorityThread(std::bind(userInputStateMachine, robot)); // User Input @@ -158,7 +157,7 @@ int main(int argc, char* argv[]) std::cout << "[l] - show plan list" << std::endl; std::cout << "[s] - stop execution of current plan" << std::endl; - // user input polling + // User input polling std::string inputBuffer; while (true) { std::cin >> inputBuffer; diff --git a/example/primitive_execution.cpp b/example/primitive_execution.cpp index 263601cb..e9772679 100644 --- a/example/primitive_execution.cpp +++ b/example/primitive_execution.cpp @@ -1,7 +1,7 @@ /** * @example primitive_execution.cpp * Execute various primitives using RDK's primitive execution API. - * @copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. + * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ @@ -17,42 +17,42 @@ #include namespace { -/** set this to true to print primitive states */ +/** Set this to true to print primitive states */ const bool k_printPtStates = false; } -// callback function for user-defined primitive state machine +/** Callback function for user-defined primitive state machine */ void primitiveStateMachine(std::shared_ptr robot) { - // log object for printing message with timestamp and coloring + // Log object for printing message with timestamp and coloring flexiv::Log log; - // data struct for system status + // Data struct for system status flexiv::SystemStatus systemStatus; - // if transitting to the next primitive + // If transitting to the next primitive bool transitPrimitive = false; - // index for iterating through various primitives + // Index for iterating through various primitives unsigned int ptIndex = 1; - // start any primitive once to pass the first transition condition check + // Start any primitive once to pass the first transition condition check robot->executePrimitive("Home()"); - // use while loop to prevent this thread from return + // Use while loop to prevent this thread from return while (true) { - // run periodic tasks at 10Hz in this thread + // Run user periodic tasks at 10Hz in this thread std::this_thread::sleep_for(std::chrono::milliseconds(100)); - // reset transition flag before checking for transition conditions + // Reset transition flag before checking for transition conditions transitPrimitive = false; - // get system status + // Get system status robot->getSystemStatus(&systemStatus); - // check for transition conditions + // Check for transition conditions for (const auto& state : systemStatus.m_ptStates) { - // parse key information from the string list for transition + // Parse key information from the string list for transition // control std::stringstream ss(state); std::string buffer; @@ -61,86 +61,76 @@ void primitiveStateMachine(std::shared_ptr robot) parsedState.push_back(buffer); } - // transition condition: "reachedTarget = 1" + // Transition condition: "reachedTarget = 1" if (parsedState.front() == "reachedTarget") { - // save the corresponding value, it should be at the end + // Save the corresponding value, it should be at the end if (std::stoi(parsedState.back()) == 1) { transitPrimitive = true; } } } - // iterate through commanding various primitives + // Iterate through commanding various primitives if (transitPrimitive) { switch (ptIndex) { case 0: { - // (1) go home - // all parameters of the "Home" primitive are optional, thus - // we can skip the parameters and the default values will be - // used + // (1) Go home, all parameters of the "Home" primitive are + // optional, thus we can skip the parameters and the default + // values will be used robot->executePrimitive("Home()"); - // execute the next primitive + // Execute the next primitive ptIndex++; break; } case 1: { - // (2) move TCP to point A in task space w.r.t world (base) - // frame the mandatory parameter "target" requires 8 values: - // [x y z roll pitch yaw reference_frame reference_point] + // (2) Move TCP to point A in world (base) frame. The + // mandatory parameter "target" requires 8 values: [x y z + // roll pitch yaw reference_frame reference_point] // unit: meters and degrees robot->executePrimitive( "MoveL(target=0.387 -0.11 0.203 90.0 0.0 180.0 WORLD " "WORLD_ORIGIN)"); - // execute the next primitive + // Execute the next primitive ptIndex++; break; } case 2: { - // (3) move TCP to point B in task space w.r.t world (base) - // frame the mandatory parameter "target" requires 8 values: - // [x y z roll pitch yaw reference_frame reference_point] - // unit: meters and degrees + // (3) Move TCP to point B in world (base) frame robot->executePrimitive( "MoveL(target=0.687 0.1 0.264 180.0 0.0 180.0 WORLD " "WORLD_ORIGIN)"); - // execute the next primitive + // Execute the next primitive ptIndex++; break; } case 3: { - // (4) move TCP to point C in task space w.r.t world (base) - // frame the mandatory parameter "target" requires 8 values: - // [x y z roll pitch yaw reference_frame reference_point] - // unit: meters and degrees + // (4) Move TCP to point C in world (base) frame robot->executePrimitive( "MoveL(target=0.387 0.5 0.1 -90.0 0.0 180.0 WORLD " "WORLD_ORIGIN)"); - // execute the next primitive + // Execute the next primitive ptIndex++; break; } case 4: { - // (4) move TCP to point C in task space w.r.t world (base) - // frame the mandatory parameter "target" requires 8 values: - // [x y z roll pitch yaw reference_frame reference_point] - // unit: meters and degrees + // (5) Move TCP to point D in world (base) frame robot->executePrimitive( "MoveL(target=0.5 0.0 0.3 180.0 0.0 180.0 WORLD " "WORLD_ORIGIN)"); - // execute the next primitive + // Execute the next primitive ptIndex++; break; } case 5: { - // repeat the moves + // Repeat the moves ptIndex = 0; break; @@ -152,12 +142,12 @@ void primitiveStateMachine(std::shared_ptr robot) } } - // print each state in the primitive states string list + // Print each state in the primitive states string list if (k_printPtStates) { for (const auto& state : systemStatus.m_ptStates) { std::cout << state << std::endl; } - // empty line + // Empty line std::cout << std::endl; } } @@ -165,12 +155,12 @@ void primitiveStateMachine(std::shared_ptr robot) int main(int argc, char* argv[]) { - // log object for printing message with timestamp and coloring + // Log object for printing message with timestamp and coloring flexiv::Log log; // Parse Parameters //============================================================================= - // check if program has 3 arguments + // Check if program has 3 arguments if (argc != 3) { log.error("Invalid program arguments. Usage: "); return 0; @@ -186,43 +176,43 @@ int main(int argc, char* argv[]) // RDK robot client auto robot = std::make_shared(); - // create data struct for storing robot states + // Create data struct for storing robot states auto robotStates = std::make_shared(); - // initialize robot interface and connect to the robot server + // Initialize robot interface and connect to the robot server robot->init(robotIP, localIP); - // wait for the connection to be established + // Wait for the connection to be established do { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } while (!robot->isConnected()); - // enable the robot, make sure the E-stop is released before enabling + // Enable the robot, make sure the E-stop is released before enabling if (robot->enable()) { log.info("Enabling robot ..."); } - // wait for the robot to become operational + // Wait for the robot to become operational do { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } while (!robot->isOperational()); log.info("Robot is now operational"); - // set mode after robot is operational + // Set mode after robot is operational robot->setMode(flexiv::MODE_PRIMITIVE_EXECUTION); - // wait for the mode to be switched + // Wait for the mode to be switched do { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } while (robot->getMode() != flexiv::MODE_PRIMITIVE_EXECUTION); // Low-priority Background Tasks //============================================================================= - // use std::thread for some non-realtime tasks, not joining + // Use std::thread for some non-realtime tasks, not joining // (non-blocking) std::thread lowPriorityThread(std::bind(primitiveStateMachine, robot)); - // block main thread + // Block main thread lowPriorityThread.join(); return 0; diff --git a/example/robot_dynamics.cpp b/example/robot_dynamics.cpp index 4d3c5dc3..ed3d49a7 100644 --- a/example/robot_dynamics.cpp +++ b/example/robot_dynamics.cpp @@ -2,7 +2,7 @@ * @example robot_dynamics.cpp * Demonstrate how to use RDK's built-in dynamics engine using APIs in * flexiv::Model. - * @copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. + * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ @@ -18,47 +18,47 @@ namespace { -// loop counter +/** Loop counter */ unsigned int g_loopCounter = 0; -// timer to measure scheduler performance +/** Timer to measure scheduler performance */ std::chrono::high_resolution_clock::time_point g_tic, g_toc; -// data to be printed in low-priority thread +/** Data to be printed in low-priority thread */ struct PrintData { int64_t loopTime; Eigen::VectorXd gravity; } g_printData; -// mutex on data to be printed in low-priority thread +/** Mutex on data to be printed in low-priority thread */ std::mutex g_printDataMutex; } -// user-defined high-priority realtime periodic task, running at 1kHz +/** User-defined high-priority realtime periodic task, running at 1kHz */ void highPriorityPeriodicTask(std::shared_ptr robotStates, std::shared_ptr robot, std::shared_ptr model) { - // mark timer start point + // Mark timer start point g_tic = std::chrono::high_resolution_clock::now(); - // get new robot states + // Get new robot states robot->getRobotStates(robotStates.get()); - // update robot model in dynamics engine + // Update robot model in dynamics engine model->updateModel(robotStates->m_q, robotStates->m_dtheta); - // get and print gravity vector + // Get and print gravity vector auto gravity = model->getGravityForce(); - // mark timer end point and get loop time + // Mark timer end point and get loop time g_toc = std::chrono::high_resolution_clock::now(); auto loopTime = std::chrono::duration_cast(g_toc - g_tic) .count(); - // save to global struct for printing in another thread + // Save to global struct for printing in another thread { std::lock_guard lock(g_printDataMutex); g_printData.loopTime = loopTime; @@ -66,19 +66,20 @@ void highPriorityPeriodicTask(std::shared_ptr robotStates, } } -// user-defined low-priority non-realtime task -// NOT strictly scheduled at a fixed rate +/** User-defined low-priority non-realtime task + * @note NOT strictly scheduled at a fixed rate + */ void lowPriorityTask() { - // wait for a while for the time interval data to be available + // Wait for a while for the time interval data to be available std::this_thread::sleep_for(std::chrono::seconds(2)); - // use while loop to prevent this thread from return + // Use while loop to prevent this thread from return while (true) { - // wake up every second to do something + // Wake up every second to do something std::this_thread::sleep_for(std::chrono::seconds(1)); - // safely read the global variable + // Safely read the global variable int loopTime; Eigen::VectorXd gravity; { @@ -87,10 +88,10 @@ void lowPriorityTask() gravity = g_printData.gravity; } - // print time interval of high-priority periodic task + // Print time interval of high-priority periodic task std::cout << "Loop time = " << loopTime << " us || "; - // print gravity + // Print gravity std::cout << std::fixed << std::setprecision(5) << "Gravity = " << gravity.transpose() << std::endl; } @@ -98,12 +99,12 @@ void lowPriorityTask() int main(int argc, char* argv[]) { - // log object for printing message with timestamp and coloring + // Log object for printing message with timestamp and coloring flexiv::Log log; // Parse Parameters //============================================================================= - // check if program has 3 arguments + // Check if program has 3 arguments if (argc != 3) { log.error("Invalid program arguments. Usage: "); return 0; @@ -116,35 +117,35 @@ int main(int argc, char* argv[]) // RDK Initialization //============================================================================= - // instantiate robot interface + // Instantiate robot interface auto robot = std::make_shared(); - // create data struct for storing robot states + // Create data struct for storing robot states auto robotStates = std::make_shared(); - // initialize robot interface and connect to the robot server + // Initialize robot interface and connect to the robot server robot->init(robotIP, localIP); - // wait for the connection to be established + // Wait for the connection to be established do { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } while (!robot->isConnected()); - // enable the robot, make sure the E-stop is released before enabling + // Enable the robot, make sure the E-stop is released before enabling if (robot->enable()) { log.info("Enabling robot ..."); } - // wait for the robot to become operational + // Wait for the robot to become operational do { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } while (!robot->isOperational()); log.info("Robot is now operational"); - // set mode after robot is operational + // Set mode after robot is operational robot->setMode(flexiv::MODE_PLAN_EXECUTION); - // wait for mode to be switched + // Wait for mode to be switched do { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } while (robot->getMode() != flexiv::MODE_PLAN_EXECUTION); @@ -154,22 +155,19 @@ int main(int argc, char* argv[]) robot->executePlanByName("PLAN-Home"); flexiv::SystemStatus systemStatus; - // wait until execution begin - do { - robot->getSystemStatus(&systemStatus); - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } while (systemStatus.m_programRunning == false); + // Wait for the plan to start + std::this_thread::sleep_for(std::chrono::seconds(1)); - // wait until execution finished + // Wait for the execution to finish do { robot->getSystemStatus(&systemStatus); std::this_thread::sleep_for(std::chrono::milliseconds(1)); } while (systemStatus.m_programRunning == true); - // put mode back to IDLE + // Put mode back to IDLE robot->setMode(flexiv::MODE_IDLE); - // wait for the mode to be switched + // Wait for the mode to be switched do { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } while (robot->getMode() != flexiv::MODE_IDLE); @@ -177,17 +175,17 @@ int main(int argc, char* argv[]) // Robot Model (Dynamics Engine) Initialization //============================================================================= auto model = std::make_shared(); - // load model from robot client + // Load model from robot client robot->loadModel(model.get()); // Low-priority Background Tasks //============================================================================= - // use std::thread for some non-realtime tasks, not joining (non-blocking) + // Use std::thread for some non-realtime tasks, not joining (non-blocking) std::thread lowPriorityThread(lowPriorityTask); // High-priority Realtime Periodic Task @ 1kHz //============================================================================= - // this is a blocking method, so all other user-defined background threads + // This is a blocking method, so all other user-defined background threads // should be spawned before this robot->start( std::bind(highPriorityPeriodicTask, robotStates, robot, model)); diff --git a/example/series_operation.cpp b/example/series_operation.cpp index 19f0ce6b..3e8cc312 100644 --- a/example/series_operation.cpp +++ b/example/series_operation.cpp @@ -2,7 +2,7 @@ * @example series_operation.cpp * Consecutively run a series of operations of different types using * various RDK APIs. - * @copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. + * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ @@ -15,49 +15,49 @@ #include namespace { -// robot DOF +/** Robot DOF */ const int k_robotDofs = 7; -// size of Cartesian pose vector [position 3x1 + rotation (quaternion) 4x1 ] +/** Size of Cartesian pose vector [position 3x1 + rotation (quaternion) 4x1 ] */ const unsigned int k_cartPoseSize = 7; -// loop period [s] +/** Loop period [s] */ const double k_loopPeiord = 0.001; -// joint impedance control gains +/** Joint impedance control gains */ const std::vector k_impedanceKp = {3000.0, 3000.0, 800.0, 800.0, 200.0, 200.0, 200.0}; const std::vector k_impedanceKd = {80.0, 80.0, 40.0, 40.0, 8.0, 8.0, 8.0}; -// joint sine-sweep trajectory amplitude and frequency +/** Joint sine-sweep trajectory amplitude and frequency */ const std::vector k_sineAmp = {0.035, 0.035, 0.035, 0.035, 0.035, 0.035, 0.035}; const std::vector k_sineFreq = {0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5}; -// TCP sine-sweep amplitude [m] +/** TCP sine-sweep amplitude [m] */ const double k_swingAmp = 0.1; -// TCP sine-sweep frequency [Hz] +/** TCP sine-sweep frequency [Hz] */ const double k_swingFreq = 0.3; -// initial joint positions +/** Initial joint positions */ std::vector g_initJointPos; -// initial TCP pose +/** Initial TCP pose */ std::vector g_initTcpPose; -// sine counter +/** Sine counter */ unsigned int g_sineCounter = 0; -// index of the current executing operation from op_list +/** Index of the current executing operation from op_list */ unsigned int g_opIndex = 0; -// time elapsed for the current operation +/** Time elapsed for the current operation */ unsigned int g_opTimer = 0; -// loop counter for 1 second +/** Loop counter for 1 second */ const unsigned int k_secToCount = 1000; -// flag for sending plan command only once +/** Flag for sending plan command only once */ bool g_isPlanSent = false; -// type of operations +/** Type of operations */ enum Operation { OP_IDLE, @@ -68,10 +68,10 @@ enum Operation OP_JOINT_TORQUE_SINE, OP_CARTESIAN_SINE, OP_FINISH, - // helper - OP_NUM = OP_FINISH - OP_IDLE + 1 + OP_NUM = OP_FINISH - OP_IDLE + 1, ///< Helper }; -// name list for printing + +/** Name list for printing */ const std::string OperationNames[OP_NUM] = {"Idle", "Stop robot", "Go home", "Multi-dof sine", "Sine-sweep with joint position control", "Sine-sweep with joint torque control", @@ -79,44 +79,42 @@ const std::string OperationNames[OP_NUM] = {"Idle", "Stop robot", "Go home", } -void resetVars(void) { } - -// callback function for realtime periodic task +/** Callback function for realtime periodic task */ void periodicTask(std::shared_ptr robotStates, std::shared_ptr robot, const std::vector& op_list, flexiv::Log& log) { - // read robot states + // Read robot states robot->getRobotStates(robotStates.get()); - // state machine on the operation list + // State machine on the operation list switch (op_list[g_opIndex]) { case OP_IDLE: { - // do nothing, transit to next operation in list + // Do nothing, transit to next operation in list g_opIndex++; - // reset operation timer + // Reset operation timer g_opTimer = 0; - // print + // Print log.info( "Running operation: " + OperationNames[op_list[g_opIndex]]); break; } case OP_STOP_ROBOT: { - // command robot to stop, call this once. It's recommended to call + // Command robot to stop, call this once. It's recommended to call // stop() when switching robot mode if (g_opTimer == 0) { robot->stop(); } - // wait for a while before checking if robot has come to a + // Wait for a while before checking if robot has come to a // complete stop if (++g_opTimer >= 1 * k_secToCount) { if (robot->isStopped()) { - // done, transit to next operation in list + // Done, transit to next operation in list g_opIndex++; - // reset operation timer + // Reset operation timer g_opTimer = 0; - // print + // Print log.info("Running operation: " + OperationNames[op_list[g_opIndex]]); } @@ -124,27 +122,27 @@ void periodicTask(std::shared_ptr robotStates, break; } case OP_GO_HOME: { - // must switch to the correct mode first + // Must switch to the correct mode first if (robot->getMode() != flexiv::MODE_PLAN_EXECUTION) { robot->setMode(flexiv::MODE_PLAN_EXECUTION); } else { - // send plan command only once + // Send plan command only once if (!g_isPlanSent) { robot->executePlanByName("PLAN-Home"); g_isPlanSent = true; } - // wait for a while before checking if robot has come to a + // Wait for a while before checking if robot has come to a // complete stop if (++g_opTimer >= 1 * k_secToCount) { if (robot->isStopped()) { - // done, transit to next operation in list + // Done, transit to next operation in list g_opIndex++; - // reset operation timer + // Reset operation timer g_opTimer = 0; - // reset flag + // Reset flag g_isPlanSent = false; - // print + // Print log.info("Running operation: " + OperationNames[op_list[g_opIndex]]); } @@ -153,25 +151,25 @@ void periodicTask(std::shared_ptr robotStates, break; } case OP_MULTIDOF_SINE: { - // must switch to the correct mode first + // Must switch to the correct mode first if (robot->getMode() != flexiv::MODE_PLAN_EXECUTION) { robot->setMode(flexiv::MODE_PLAN_EXECUTION); } else { - // send plan command only once + // Send plan command only once if (!g_isPlanSent) { robot->executePlanByName("MultiDOFSineTest_A5"); g_isPlanSent = true; } - // wait for operation period to timeout + // Wait for operation period to timeout if (++g_opTimer >= 20 * k_secToCount) { - // done, transit to next operation in list + // Done, transit to next operation in list g_opIndex++; - // reset operation timer + // Reset operation timer g_opTimer = 0; - // reset flag + // Reset flag g_isPlanSent = false; - // print + // Print log.info("Running operation: " + OperationNames[op_list[g_opIndex]]); } @@ -179,21 +177,21 @@ void periodicTask(std::shared_ptr robotStates, break; } case OP_JOINT_POSITION_SINE: { - // must switch to the correct mode first + // Must switch to the correct mode first if (robot->getMode() != flexiv::MODE_JOINT_POSITION) { robot->setMode(flexiv::MODE_JOINT_POSITION); - // set initial joint position + // Set initial joint position if (robotStates->m_q.size() == k_robotDofs) { g_initJointPos = robotStates->m_q; } else { log.error("Invalid size of received joint position"); - // stop robot and terminate program + // Stop robot and terminate program robot->stop(); exit(1); } - // reset sine counter + // Reset sine counter g_sineCounter = 0; } else { std::vector targetPosition(k_robotDofs, 0); @@ -208,20 +206,20 @@ void periodicTask(std::shared_ptr robotStates, * k_loopPeiord); } - // increment sine counter + // Increment sine counter g_sineCounter++; - // send command + // Send command robot->streamJointPosition( targetPosition, targetVelocity, targetAcceleration); - // wait for operation period to timeout + // Wait for operation period to timeout if (++g_opTimer >= 10 * k_secToCount) { - // done, transit to next operation in list + // Done, transit to next operation in list g_opIndex++; - // reset operation timer + // Reset operation timer g_opTimer = 0; - // print + // Print log.info("Running operation: " + OperationNames[op_list[g_opIndex]]); } @@ -229,20 +227,20 @@ void periodicTask(std::shared_ptr robotStates, break; } case OP_JOINT_TORQUE_SINE: { - // must switch to the correct mode first + // Must switch to the correct mode first if (robot->getMode() != flexiv::MODE_JOINT_TORQUE) { robot->setMode(flexiv::MODE_JOINT_TORQUE); - // set initial joint position + // Set initial joint position if (robotStates->m_q.size() == k_robotDofs) { g_initJointPos = robotStates->m_q; } else { log.error("Invalid size of received joint position"); - // stop robot and terminate program + // Stop robot and terminate program robot->stop(); exit(1); } - // reset sine counter + // Reset sine counter g_sineCounter = 0; } else { std::vector targetPosition(k_robotDofs, 0); @@ -254,10 +252,10 @@ void periodicTask(std::shared_ptr robotStates, * k_loopPeiord); } - // increment sine counter + // Increment sine counter g_sineCounter++; - // impedance control on all joints + // Impedance control on all joints std::vector torqueDesired(k_robotDofs, 0); for (size_t i = 0; i < k_robotDofs; ++i) { torqueDesired[i] @@ -266,16 +264,16 @@ void periodicTask(std::shared_ptr robotStates, - k_impedanceKd[i] * robotStates->m_dtheta[i]; } - // send command + // Send command robot->streamJointTorque(torqueDesired, true); - // wait for operation period to timeout + // Wait for operation period to timeout if (++g_opTimer >= 10 * k_secToCount) { - // done, transit to next operation in list + // Done, transit to next operation in list g_opIndex++; - // reset operation timer + // Reset operation timer g_opTimer = 0; - // print + // Print log.info("Running operation: " + OperationNames[op_list[g_opIndex]]); } @@ -283,45 +281,45 @@ void periodicTask(std::shared_ptr robotStates, break; } case OP_CARTESIAN_SINE: { - // must switch to the correct mode first + // Must switch to the correct mode first if (robot->getMode() != flexiv::MODE_CARTESIAN_IMPEDANCE) { robot->setMode(flexiv::MODE_CARTESIAN_IMPEDANCE); - // set initial TCP pose + // Set initial TCP pose if (robotStates->m_tcpPose.size() == k_cartPoseSize) { g_initTcpPose = robotStates->m_tcpPose; } else { log.error("Invalid size of received TCP pose"); - // stop robot and terminate program + // Stop robot and terminate program robot->stop(); exit(1); } - // reset sine counter + // Reset sine counter g_sineCounter = 0; } else { auto targetTcpPose = g_initTcpPose; - // use target TCP velocity and acceleration = 0 + // Use target TCP velocity and acceleration = 0 std::vector targetTcpVel(6, 0); std::vector targetTcpAcc(6, 0); - // sine-sweep X direction + // Sine-sweep X direction targetTcpPose[1] = g_initTcpPose[1] + k_swingAmp * sin(2 * M_PI * k_swingFreq * g_sineCounter * k_loopPeiord); - // increment sine counter + // Increment sine counter g_sineCounter++; - // send command + // Send command robot->streamTcpPose(targetTcpPose, targetTcpVel, targetTcpAcc); - // wait for operation period to timeout + // Wait for operation period to timeout if (++g_opTimer >= 20 * k_secToCount) { - // done, transit to next operation in list + // Done, transit to next operation in list g_opIndex++; - // reset operation timer + // Reset operation timer g_opTimer = 0; - // print + // Print log.info("Running operation: " + OperationNames[op_list[g_opIndex]]); } @@ -329,7 +327,7 @@ void periodicTask(std::shared_ptr robotStates, break; } case OP_FINISH: { - // repeat the whole sequence + // Repeat the whole sequence g_opIndex = 0; break; } @@ -340,12 +338,12 @@ void periodicTask(std::shared_ptr robotStates, int main(int argc, char* argv[]) { - // log object for printing message with timestamp and coloring + // Log object for printing message with timestamp and coloring flexiv::Log log; // Parse Parameters //============================================================================= - // check if program has 3 arguments + // Check if program has 3 arguments if (argc != 3) { log.error("Invalid program arguments. Usage: "); return 0; @@ -358,26 +356,26 @@ int main(int argc, char* argv[]) // RDK Initialization //============================================================================= - // instantiate robot interface + // Instantiate robot interface auto robot = std::make_shared(); - // create data struct for storing robot states + // Create data struct for storing robot states auto robotStates = std::make_shared(); - // initialize robot interface and connect to the robot server + // Initialize robot interface and connect to the robot server robot->init(robotIP, localIP); - // wait for the connection to be established + // Wait for the connection to be established do { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } while (!robot->isConnected()); - // enable the robot, make sure the E-stop is released before enabling + // Enable the robot, make sure the E-stop is released before enabling if (robot->enable()) { log.info("Enabling robot ..."); } - // wait for the robot to become operational + // Wait for the robot to become operational do { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } while (!robot->isOperational()); @@ -385,7 +383,7 @@ int main(int argc, char* argv[]) // List of Operations //============================================================================= - // it is recommended to call stop() when switching robot mode + // It is recommended to call stop() when switching robot mode std::vector op_list = { OP_IDLE, OP_GO_HOME, @@ -408,7 +406,7 @@ int main(int argc, char* argv[]) // High-priority Realtime Periodic Task @ 1kHz //============================================================================= - // this is a blocking method, so all other user-defined background threads + // This is a blocking method, so all other user-defined background threads // should be spawned before this robot->start(std::bind(periodicTask, robotStates, robot, op_list, log)); diff --git a/example/visualization.cpp b/example/visualization.cpp index 83e5c737..4d91dd87 100644 --- a/example/visualization.cpp +++ b/example/visualization.cpp @@ -2,7 +2,7 @@ * @example visualization.cpp * Run the RDK's integrated visualizer with the robot TCP doing sine-sweep * movement. - * @copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. + * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ @@ -15,56 +15,56 @@ namespace { -// size of Cartesian pose vector [position 3x1 + rotation (quaternion) 4x1 ] +/** Size of Cartesian pose vector [position 3x1 + rotation (quaternion) 4x1 ] */ const unsigned int k_cartPoseSize = 7; -// RT loop period [sec] +/** RT loop period [sec] */ const double k_loopPeiord = 0.001; -// TCP sine-sweep amplitude [m] +/** TCP sine-sweep amplitude [m] */ const double k_swingAmp = 0.1; -// TCP sine-sweep frequency [Hz] +/** TCP sine-sweep frequency [Hz] */ const double k_swingFreq = 0.3; -// initial Cartesian-space pose (position + rotation) of robot TCP +/** Initial Cartesian-space pose (position + rotation) of robot TCP */ std::vector g_initTcpPose; -// current Cartesian-space pose (position + rotation) of robot TCP +/** Current Cartesian-space pose (position + rotation) of robot TCP */ std::vector g_currentTcpPose; -// flag whether initial Cartesian position is set +/** Flag whether initial Cartesian position is set */ bool g_isInitPoseSet = false; -// loop counter +/** Loop counter */ unsigned int g_loopCounter = 0; -// joint positions used by visualizer +/** Joint positions used by visualizer */ std::vector g_jointPositions; } -// callback function for realtime periodic task +/** Callback function for realtime periodic task */ void periodicTask(std::shared_ptr robotStates, std::shared_ptr robot) { - // read robot states + // Read robot states robot->getRobotStates(robotStates.get()); - // use target TCP velocity and acceleration = 0 + // Use target TCP velocity and acceleration = 0 std::vector tcpVel(6, 0); std::vector tcpAcc(6, 0); - // set initial TCP pose + // Set initial TCP pose if (!g_isInitPoseSet) { - // check vector size before saving + // Check vector size before saving if (robotStates->m_tcpPose.size() == k_cartPoseSize) { g_initTcpPose = robotStates->m_tcpPose; g_currentTcpPose = g_initTcpPose; g_isInitPoseSet = true; } } - // run control only after initial pose is set + // Run control only after initial pose is set else { g_currentTcpPose[1] = g_initTcpPose[1] + k_swingAmp @@ -73,35 +73,36 @@ void periodicTask(std::shared_ptr robotStates, robot->streamTcpPose(g_currentTcpPose, tcpVel, tcpAcc); } - // save joint positions to global variable by visualizer, skipping mutex + // Save joint positions to global variable by visualizer, skipping mutex g_jointPositions = robotStates->m_q; g_loopCounter++; } +/** Low-priority periodic task for visualizer stuff */ void visualizerTask(std::shared_ptr visualizer) { - // wait a while for the data to start streaming + // Wait a while for the data to start streaming std::this_thread::sleep_for(std::chrono::seconds(2)); - // use while loop to prevent this thread from return + // Use while loop to prevent this thread from return while (true) { - // run periodic tasks at 100Hz in this thread + // Run periodic tasks at 100Hz in this thread std::this_thread::sleep_for(std::chrono::milliseconds(10)); - // update visualizer with joint positions + // Update visualizer with joint positions visualizer->update(g_jointPositions); } } int main(int argc, char* argv[]) { - // log object for printing message with timestamp and coloring + // Log object for printing message with timestamp and coloring flexiv::Log log; // Parse Parameters //============================================================================= - // check if program has 3 arguments + // Check if program has 3 arguments if (argc != 4) { log.error( "Invalid program arguments. Usage: " @@ -119,35 +120,35 @@ int main(int argc, char* argv[]) // RDK Initialization //============================================================================= - // instantiate robot interface + // Instantiate robot interface auto robot = std::make_shared(); - // create data struct for storing robot states + // Create data struct for storing robot states auto robotStates = std::make_shared(); - // initialize robot interface and connect to the robot server + // Initialize robot interface and connect to the robot server robot->init(robotIP, localIP); - // wait for the connection to be established + // Wait for the connection to be established do { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } while (!robot->isConnected()); - // enable the robot, make sure the E-stop is released before enabling + // Enable the robot, make sure the E-stop is released before enabling if (robot->enable()) { log.info("Enabling robot ..."); } - // wait for the robot to become operational + // Wait for the robot to become operational do { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } while (!robot->isOperational()); log.info("Robot is now operational"); - // set mode after robot is operational + // Set mode after robot is operational robot->setMode(flexiv::MODE_CARTESIAN_IMPEDANCE); - // wait for the mode to be switched + // Wait for the mode to be switched do { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } while (robot->getMode() != flexiv::MODE_CARTESIAN_IMPEDANCE); @@ -159,13 +160,13 @@ int main(int argc, char* argv[]) // Low-priority Background Tasks //============================================================================= - // use std::thread for some non-realtime tasks, not joining + // Use std::thread for some non-realtime tasks, not joining // (non-blocking) std::thread lowPriorityThread(std::bind(visualizerTask, visualizer)); // High-priority Realtime Periodic Task @ 1kHz //============================================================================= - // this is a blocking method, so all other user-defined background threads + // This is a blocking method, so all other user-defined background threads // should be spawned before this robot->start(std::bind(periodicTask, robotStates, robot)); diff --git a/example_py/auto_recovery.py b/example_py/auto_recovery.py new file mode 100644 index 00000000..d53c39a3 --- /dev/null +++ b/example_py/auto_recovery.py @@ -0,0 +1,71 @@ +#!/usr/bin/env python + +"""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::Mode::MODE_AUTO_RECOVERY for more details. +""" + +__copyright__ = "Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved." +__author__ = "Flexiv" + +import time +import argparse + +# Import Flexiv RDK Python library +# fmt: off +import sys +sys.path.insert(0, "../lib/") +import flexivrdk +# fmt: on + + +def main(): + # Parse Arguments + # ============================================================================= + argparser = argparse.ArgumentParser() + argparser.add_argument('robot_ip', help='IP address of the robot server') + argparser.add_argument('local_ip', help='IP address of the workstation PC') + args = argparser.parse_args() + + # RDK Initialization + # ============================================================================= + # Some alias + robot = flexivrdk.Robot() + mode = flexivrdk.Mode + + # Initialize connection with robot server + robot.init(args.robot_ip, args.local_ip) + + # Wait for the connection to be established + while not robot.isConnected(): + time.sleep(1) + + # Enable the robot, make sure the E-stop is released before enabling + if robot.enable(): + print("Enabling robot ...") + + # 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 + 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 + if robot.isRecoveryState(): + robot.startAutoRecovery() + # block forever, must reboot the robot and restart user program after auto + # recovery is done + while True: + time.sleep(1) + # Otherwise the system is normal, do nothing + else: + print("Robot system is not in recovery state, nothing to be done, exiting ...") + + +if __name__ == "__main__": + main() diff --git a/example_py/display_robot_states.py b/example_py/display_robot_states.py new file mode 100644 index 00000000..3e117760 --- /dev/null +++ b/example_py/display_robot_states.py @@ -0,0 +1,81 @@ +#!/usr/bin/env python + +"""display_robot_states.py + +Print received robot states. +""" + +__copyright__ = "Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved." +__author__ = "Flexiv" + +import time +import argparse + +# Import Flexiv RDK Python library +# fmt: off +import sys +sys.path.insert(0, "../lib/") +import flexivrdk +# fmt: on + + +def main(): + # Parse Arguments + # ============================================================================= + argparser = argparse.ArgumentParser() + argparser.add_argument('robot_ip', help='IP address of the robot server') + argparser.add_argument('local_ip', help='IP address of the workstation PC') + args = argparser.parse_args() + + # RDK Initialization + # ============================================================================= + # Some alias + robot = flexivrdk.Robot() + mode = flexivrdk.Mode + robot_states = flexivrdk.RobotStates() + + # Initialize connection with robot server + robot.init(args.robot_ip, args.local_ip) + + # Wait for the connection to be established + while not robot.isConnected(): + time.sleep(1) + + # Enable the robot, make sure the E-stop is released before enabling + if robot.enable(): + print("Enabling robot ...") + + # Wait for the robot to become operational + while not robot.isOperational(): + time.sleep(1) + print("Robot is now operational") + + # Set mode after robot is operational + robot.setMode(mode.MODE_IDLE) + + # Application-specific Code + # ============================================================================= + while True: + robot.getRobotStates(robot_states) + print("q: ", robot_states.m_q) + print("theta: ", robot_states.m_theta) + print("dq: ", robot_states.m_dq) + print("dtheta: ", robot_states.m_dtheta) + print("tau: ", robot_states.m_tau) + print("tau_des: ", robot_states.m_tauDes) + print("tau_dot: ", robot_states.m_tauDot) + print("tau_ext: ", robot_states.m_tauExt) + print("tcp_pose: ", robot_states.m_tcpPose) + print("tcp_pose_d: ", robot_states.m_tcpPoseDes) + print("tcp_velocity: ", robot_states.m_tcpVel) + print("camera_pose: ", robot_states.m_camPose) + print("flange_pose: ", robot_states.m_flangePose) + print("end_link_pose: ", robot_states.m_endLinkPose) + print("F_ext_tcp_frame: ", robot_states.m_extForceInTcpFrame) + print("F_ext_base_frame: ", robot_states.m_extForceInBaseFrame) + print("==" * 30) + time.sleep(1) + + +if __name__ == "__main__": + main() diff --git a/example_py/nrt_cartesian_impedance_control.py b/example_py/nrt_cartesian_impedance_control.py new file mode 100644 index 00000000..74015c91 --- /dev/null +++ b/example_py/nrt_cartesian_impedance_control.py @@ -0,0 +1,115 @@ +#!/usr/bin/env python + +"""nrt_cartesian_impedance_control.py + +Run non-real-time Cartesian impedance control to hold or sine-sweep the robot TCP. +""" + +__copyright__ = "Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved." +__author__ = "Flexiv" + +import time +import math +import argparse + +# Import Flexiv RDK Python library +# fmt: off +import sys +sys.path.insert(0, "../lib/") +import flexivrdk +# fmt: on + + +def main(): + # Parse Arguments + # ============================================================================= + argparser = argparse.ArgumentParser() + argparser.add_argument('robot_ip', help='IP address of the robot server') + argparser.add_argument('local_ip', help='IP address of the workstation PC') + argparser.add_argument( + 'motion_type', help='Accepted motion types: hold, sine-sweep') + argparser.add_argument( + 'frequency', help='Command frequency, 1 to 1000 [Hz]') + args = argparser.parse_args() + + # Convert string to number + frequency = float(args.frequency) + + # Check if arguments are valid + assert (args.motion_type == "hold" or args.motion_type == + "sine-sweep"), "Invalid input" + assert (frequency >= 1 and frequency <= 1000), "Invalid input" + + # RDK Initialization + # ============================================================================= + # Some alias + robot = flexivrdk.Robot() + mode = flexivrdk.Mode + robot_states = flexivrdk.RobotStates() + system_status = flexivrdk.SystemStatus() + + # Initialize connection with robot server + robot.init(args.robot_ip, args.local_ip) + + # Wait for the connection to be established + while not robot.isConnected(): + time.sleep(1) + + # Enable the robot, make sure the E-stop is released before enabling + if robot.enable(): + print("Enabling robot ...") + + # Wait for the robot to become operational + while not robot.isOperational(): + time.sleep(1) + print("Robot is now operational") + + # Set mode after robot is operational + robot.setMode(mode.MODE_CARTESIAN_IMPEDANCE_NRT) + + # Wait for the mode to be switched + while (robot.getMode() != mode.MODE_CARTESIAN_IMPEDANCE_NRT): + time.sleep(1) + + # Application-specific Code + # ============================================================================= + period = 1.0/frequency + loop_time = 0 + print("Sending command to robot at", frequency, + "Hz, or", period, "seconds interval") + + # Use current robot TCP pose as initial pose + robot.getRobotStates(robot_states) + init_pose = robot_states.m_tcpPose.copy() + print("Initial pose set to: ", init_pose) + + # Initialize target vectors + target_pose = init_pose.copy() + max_wrench = [100.0, 100.0, 100.0, 30.0, 30.0, 30.0] + + # TCP sine-sweep amplitude [m] + SWING_AMP = 0.1 + + # TCP sine-sweep frequency [Hz] + SWING_FREQ = 0.3 + + # Send command periodically at user-specified frequency + while True: + # Use sleep to control loop period + time.sleep(period) + + # Sine-sweep TCP along y axis + if args.motion_type == "sine-sweep": + target_pose[1] = init_pose[1] + SWING_AMP * \ + math.sin(2 * math.pi * SWING_FREQ * loop_time) + # Otherwise robot TCP will hold at initial pose + + # Send command + robot.sendTcpPose(target_pose, max_wrench) + + # Increment loop time + loop_time += period + + +if __name__ == "__main__": + main() diff --git a/example_py/nrt_joint_position_control.py b/example_py/nrt_joint_position_control.py new file mode 100644 index 00000000..9ed42953 --- /dev/null +++ b/example_py/nrt_joint_position_control.py @@ -0,0 +1,124 @@ +#!/usr/bin/env python + +"""nrt_joint_position_control.py + +Run non-real-time joint position control to hold or sine-sweep all joints. +""" + +__copyright__ = "Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved." +__author__ = "Flexiv" + +import time +import math +import argparse + +# Import Flexiv RDK Python library +# fmt: off +import sys +sys.path.insert(0, "../lib/") +import flexivrdk +# fmt: on + + +def main(): + # Parse Arguments + # ============================================================================= + argparser = argparse.ArgumentParser() + argparser.add_argument('robot_ip', help='IP address of the robot server') + argparser.add_argument('local_ip', help='IP address of the workstation PC') + argparser.add_argument( + 'motion_type', help='Accepted motion types: hold, sine-sweep') + argparser.add_argument( + 'frequency', help='Command frequency, 1 to 1000 [Hz]') + args = argparser.parse_args() + + # Convert string to number + frequency = float(args.frequency) + + # Check if arguments are valid + assert (args.motion_type == "hold" or args.motion_type == + "sine-sweep"), "Invalid input" + assert (frequency >= 1 and frequency <= 1000), "Invalid input" + + # RDK Initialization + # ============================================================================= + # Some alias + robot = flexivrdk.Robot() + mode = flexivrdk.Mode + robot_states = flexivrdk.RobotStates() + system_status = flexivrdk.SystemStatus() + + # Initialize connection with robot server + robot.init(args.robot_ip, args.local_ip) + + # Wait for the connection to be established + while not robot.isConnected(): + time.sleep(1) + + # Enable the robot, make sure the E-stop is released before enabling + if robot.enable(): + print("Enabling robot ...") + + # Wait for the robot to become operational + while not robot.isOperational(): + time.sleep(1) + print("Robot is now operational") + + # Set mode after robot is operational + robot.setMode(mode.MODE_JOINT_POSITION_NRT) + + # Wait for the mode to be switched + while (robot.getMode() != mode.MODE_JOINT_POSITION_NRT): + time.sleep(1) + + # Application-specific Code + # ============================================================================= + period = 1.0/frequency + loop_time = 0 + print("Sending command to robot at", frequency, + "Hz, or", period, "seconds interval") + + # Use current robot joint positions as initial positions + robot.getRobotStates(robot_states) + init_pos = robot_states.m_q.copy() + print("Initial positions set to: ", init_pos) + + # Initialize target vectors + DOF = len(robot_states.m_q) + target_pos = init_pos.copy() + target_vel = [0.0] * DOF + target_acc = [0.0] * DOF + + # Joint motion constraints + MAX_VEL = [2.0] * DOF + MAX_ACC = [3.0] * DOF + MAX_JERK = [20.0] * DOF + + # Joint sine-sweep amplitude [rad] + SWING_AMP = 0.1 + + # TCP sine-sweep frequency [Hz] + SWING_FREQ = 0.3 + + # Send command periodically at user-specified frequency + while True: + # Use sleep to control loop period + time.sleep(period) + + # Sine-sweep all joints + if args.motion_type == "sine-sweep": + for i in range(DOF): + target_pos[i] = init_pos[i] + SWING_AMP * \ + math.sin(2 * math.pi * SWING_FREQ * loop_time) + # Otherwise all joints will hold at initial positions + + # Send command + robot.sendJointPosition( + target_pos, target_vel, target_acc, MAX_VEL, MAX_ACC, MAX_JERK) + + # Increment loop time + loop_time += period + + +if __name__ == "__main__": + main() diff --git a/example_py/plan_execution.py b/example_py/plan_execution.py new file mode 100644 index 00000000..7242981c --- /dev/null +++ b/example_py/plan_execution.py @@ -0,0 +1,93 @@ +#!/usr/bin/env python + +"""plan_execution.py + +Select a plan from a list to execute using RDK's plan execution API. +""" + +__copyright__ = "Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved." +__author__ = "Flexiv" + +import time +import argparse + +# Import Flexiv RDK Python library +# fmt: off +import sys +sys.path.insert(0, "../lib/") +import flexivrdk +# fmt: on + + +def main(): + # Parse Arguments + # ============================================================================= + argparser = argparse.ArgumentParser() + argparser.add_argument('robot_ip', help='IP address of the robot server') + argparser.add_argument('local_ip', help='IP address of the workstation PC') + args = argparser.parse_args() + + # RDK Initialization + # ============================================================================= + # Some alias + robot = flexivrdk.Robot() + mode = flexivrdk.Mode + system_status = flexivrdk.SystemStatus() + + # Initialize connection with robot server + robot.init(args.robot_ip, args.local_ip) + + # Wait for the connection to be established + while not robot.isConnected(): + time.sleep(1) + + # Enable the robot, make sure the E-stop is released before enabling + if robot.enable(): + print("Enabling robot ...") + + # Wait for the robot to become operational + while not robot.isOperational(): + time.sleep(1) + print("Robot is now operational") + + # Set mode after robot is operational + robot.setMode(mode.MODE_PLAN_EXECUTION) + + # Wait for the mode to be switched + while (robot.getMode() != mode.MODE_PLAN_EXECUTION): + time.sleep(1) + + # Application-specific Code + # ============================================================================= + while True: + # Get user input + input_case = int(input( + "Choose an action:\n \ + [1] Get plan list \n \ + [2] Execute plan by index \n \ + [3] Stop the current plan execution \n" + ) + ) + + # Check if user input is valid + assert (input_case >= 1 and input_case <= 3), "Invalid input" + + # Get plan list + if input_case == 1: + plan_list = robot.getPlanNameList() + time.sleep(1) + for i in range(len(plan_list)): + print("[" + str(i) + "]", plan_list[i]) + + # Execute plan by index + elif input_case == 2: + index = int(input("Enter plan index to execute:\n")) + robot.executePlanByIndex(index) + + # Stop the current plan execution + elif input_case == 3: + robot.stop() + + +if __name__ == "__main__": + main() diff --git a/example_py/primitive_execution.py b/example_py/primitive_execution.py new file mode 100644 index 00000000..8361a157 --- /dev/null +++ b/example_py/primitive_execution.py @@ -0,0 +1,149 @@ +#!/usr/bin/env python + +"""primitive_execution.py + +Execute various primitives using RDK's primitive execution API. +""" + +__copyright__ = "Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved." +__author__ = "Flexiv" + +import time +import argparse + +# Import Flexiv RDK Python library +# fmt: off +import sys +sys.path.insert(0, "../lib/") +import flexivrdk +# fmt: on + + +def main(): + # Parse Arguments + # ============================================================================= + argparser = argparse.ArgumentParser() + argparser.add_argument('robot_ip', help='IP address of the robot server') + argparser.add_argument('local_ip', help='IP address of the workstation PC') + args = argparser.parse_args() + + # RDK Initialization + # ============================================================================= + # Some alias + robot = flexivrdk.Robot() + mode = flexivrdk.Mode + system_status = flexivrdk.SystemStatus() + + # Initialize connection with robot server + robot.init(args.robot_ip, args.local_ip) + + # Wait for the connection to be established + while not robot.isConnected(): + time.sleep(1) + + # Enable the robot, make sure the E-stop is released before enabling + if robot.enable(): + print("Enabling robot ...") + + # Wait for the robot to become operational + while not robot.isOperational(): + time.sleep(1) + print("Robot is now operational") + + # Set mode after robot is operational + robot.setMode(mode.MODE_PRIMITIVE_EXECUTION) + + # Wait for the mode to be switched + while (robot.getMode() != mode.MODE_PRIMITIVE_EXECUTION): + time.sleep(1) + + # Application-specific Code + # ============================================================================= + # Flag to transit to the next primitive + transit_primitive = False + + # Index for iterating through various primitives + pt_index = 1 + + # Go home first + robot.executePrimitive("Home()") + + while True: + # Run user periodic task at 10Hz + time.sleep(0.1) + + # Reset flag before checking for transition conditions + transit_primitive = False + + # Get system status which contains primitive states + robot.getSystemStatus(system_status) + + # Check for transition condition "reachedTarget = 1" + for state in system_status.m_ptStates: + # Split the state sentence into words + words = state.split() + + # Check if this state is "reachedTarget" + if words[0] == "reachedTarget": + # Check if the state value is 1 + if words[-1] == "1": + # Set transition flag + transit_primitive = True + + # Iterate through various primitives if the previous is finished + if transit_primitive: + if pt_index == 0: + # (1) Go home, all parameters of the "Home" primitive are + # optional, thus we can skip the parameters and the default + # values will be used + robot.executePrimitive("Home()") + + # Execute the next primitive + pt_index += 1 + + elif pt_index == 1: + # (2) Move TCP to point A in world (base) frame. The + # mandatory parameter "target" requires 8 values: [x y z + # roll pitch yaw reference_frame reference_point] + # unit: meters and degrees + robot.executePrimitive( + "MoveL(target=0.387 -0.11 0.203 90.0 0.0 180.0 WORLD " + "WORLD_ORIGIN)") + + # Execute the next primitive + pt_index += 1 + + elif pt_index == 2: + # (3) Move TCP to point B in world (base) frame + robot.executePrimitive( + "MoveL(target=0.687 0.1 0.264 180.0 0.0 180.0 WORLD " + "WORLD_ORIGIN)") + + # Execute the next primitive + pt_index += 1 + + elif pt_index == 3: + # (4) Move TCP to point C in world (base) frame + robot.executePrimitive( + "MoveL(target=0.387 0.5 0.1 -90.0 0.0 180.0 WORLD " + "WORLD_ORIGIN)") + + # Execute the next primitive + pt_index += 1 + + elif pt_index == 4: + # (5) Move TCP to point D in world(base) frame + robot.executePrimitive( + "MoveL(target=0.5 0.0 0.3 180.0 0.0 180.0 WORLD " + "WORLD_ORIGIN)") + + # Execute the next primitive + pt_index += 1 + + elif pt_index == 5: + # Repeat the moves + pt_index = 0 + + +if __name__ == "__main__": + main() diff --git a/include/Mode.hpp b/include/Mode.hpp index 07b48e85..a409a924 100644 --- a/include/Mode.hpp +++ b/include/Mode.hpp @@ -22,32 +22,42 @@ enum Mode MODE_IDLE, ///< The robot holds position and waits for commands /** - * @note Real-time control mode running at 1kHz + * @note Receive and execute user commands at 1kHz */ - MODE_JOINT_POSITION, ///< Joint position control + MODE_JOINT_TORQUE, ///< Real-time joint torque control /** - * @note Real-time control mode running at 1kHz + * @note Receive and execute user commands at 1kHz */ - MODE_JOINT_TORQUE, ///< Joint torque control + MODE_JOINT_POSITION, ///< Real-time joint position control /** - * @note Non-realtime control mode. The user selects a plan and executes it. - * Upon completion, the user can send another plan + * @note Receive and execute user commands at < 1kHz + */ + MODE_JOINT_POSITION_NRT, ///< Non-real-time joint position control + + /** + * @note Non-real-time control mode. The user selects a plan and executes + * it. Upon completion, the user can send another plan */ MODE_PLAN_EXECUTION, ///< Execute the plan specified by user /** - * @note Non-realtime control mode. The user sends a primitive command with + * @note Non-real-time control mode. The user sends a primitive command with * the name and the associated parameters and executes it. Upon completion, * the user can send another primitive command */ MODE_PRIMITIVE_EXECUTION, ///< Execute the primitive specified by user /** - * @note Real-time control mode running at 1kHz + * @note Receive and execute user commands at 1kHz + */ + MODE_CARTESIAN_IMPEDANCE, ///< Real-time Cartesian impedance control + + /** + * @note Receive and execute user commands at < 1kHz */ - MODE_CARTESIAN_IMPEDANCE, ///< Cartesian impedance control + MODE_CARTESIAN_IMPEDANCE_NRT, ///< Non-realtime Cartesian impedance control }; } /* namespace flexiv */ diff --git a/include/Robot.hpp b/include/Robot.hpp index c24f80c5..ae5d6fdb 100644 --- a/include/Robot.hpp +++ b/include/Robot.hpp @@ -32,66 +32,74 @@ class Robot //============================ SYSTEM METHODS ============================= /** - * @brief Initialize robot client and establish connection with server - * @param[in] serverAddr The IP address of RDK server (remote) - * @param[in] localAddr The IP address of RDK client (local) + * @brief Initialize RDK services and establish connection with the robot + * server. + * @param[in] serverIP IP address of the robot server (remote) + * @param[in] localIP IP address of the workstation PC (local) * @return True: success, false: failed + * @version C++, Python */ - bool init(const std::string& serverAddr, const std::string& localAddr); + bool init(const std::string& serverIP, const std::string& localIP); /** - * @brief Load the model library from the robot + * @brief Load the model library from the robot. * @param[out] model Pointer to the flexiv::Model object that gets updated * @param[in] gravityEarth Earth's gravity vector in base frame. Default * to \f$ [0.0, 0.0, -9.81]^T~[m/s^2] \f$ * @return True: success, false: failed * @note Must call this method before using the flexiv::Model library + * @version C++ */ bool loadModel(Model* model, const Eigen::Vector3d& gravityEarth = Eigen::Vector3d(0.0, 0.0, -9.81)); /** * @brief Enable the robot, if E-stop is released and there's no fault, the - * robot will release brakes, and becomes operational a few seconds later + * robot will release brakes, and becomes operational a few seconds later. * @return True: success, false: failed * @note Call init() before calling this method * @warning Only call this method when needed, avoid calling it periodically + * @version C++, Python */ bool enable(void); /** - * @brief Start running user-defined high-priority control task at 1000 Hz + * @brief Start running user-defined high-priority control task at 1000 Hz. * @param[in] callback Callback function with user code * @return True: success, false: failed * @note This method will block the thread from which this function is * called, so user needs to spawn all other background threads, if any, * before calling this method + * @version C++ */ bool start(std::function&& callback); /** - * @brief Stop the robot + * @brief Stop the robot. * @return True: attempt successful, false: attempt failed * @note This will transit the robot mode to Idle * @warning It is recommended to call this method before switching from one * robot mode to another + * @version C++, Python */ bool stop(void); /** - * @brief Check if the robot is normally operational + * @brief Check if the robot is normally operational. * @return True: operational, false: not operational + * @version C++, Python */ bool isOperational(void) const; /** - * @brief Check status of the connection with the robot server + * @brief Check if the connection with the robot server is established. * @return True: connected, false: disconnected + * @version C++, Python */ bool isConnected(void) const; /** - * @brief Check if the robot system is in recovery state + * @brief Check if the robot system is in recovery state. * @return True: in recovery state, false: not in recovery state * @note The robot system will enter recovery state if it needs to recover * from joint position limit violation (a critical system fault that @@ -100,54 +108,62 @@ class Robot * allowed range). Refer to user manual for more details about system * recovery state. * @note Use startAutoRecovery() to carry out recovery operation + * @version C++, Python */ bool isRecoveryState(void) const; /** - * @brief Check if the heartbeat signal between server and client has - * timeout + * @brief Check if the connection with the robot server has timeout, + * according to the heartbeat singal monitoring. * @return True: timeout, false: not timeout + * @version C++, Python */ - bool timeout(void) const; + bool isTimeout(void) const; /** - * @brief Try establishing connection with the server + * @brief Try establishing connection with the robot server. * @return True: success, false: failed + * @version C++, Python */ bool connect(void); /** - * @brief Disconnect with the server + * @brief Disconnect with the robot server. + * @version C++, Python */ void disconnect(void); /** - * @brief Check if the robot is in fault state + * @brief Check if the robot is in fault state. * @return True: robot has fault, false: robot normal + * @version C++, Python */ bool isFault(void) const; /** - * @brief Clear minor fault of the robot + * @brief Clear minor fault of the robot. * @return True: success, false: failed + * @version C++, Python */ bool clearFault(void); //=========================== CONTROL METHODS ============================== /** - * @brief Set robot mode, call it after initialization + * @brief Set robot mode, call it after initialization. * @param[in] mode Mode of the robot, see flexiv::Mode * @return True: success, false: failed * @warning Only call this method when needed, avoid periodic calling. To * avoid unexpected behavior, it's recommended to call stop() and then check * if the robot has come to a complete stop using isStopped() before * switching robot's control mode + * @version C++, Python */ bool setMode(Mode mode); /** - * @brief Get the current robot mode + * @brief Get the current robot mode. * @return Mode of the robot, see flexiv::Mode + * @version C++, Python */ Mode getMode(void) const; @@ -157,6 +173,7 @@ class Robot * @param[out] robotStates RobotStates data struct * @return True: success, false: failed * @note Call this method in the periodic loop + * @version C++, Python */ bool getRobotStates(RobotStates* robotStates); @@ -165,72 +182,79 @@ class Robot * status, etc. * @param[out] systemStatus SystemStatus data struct * @return True: success, false: failed + * @version C++, Python */ bool getSystemStatus(SystemStatus* systemStatus); /** - * @brief Select the plan to execute using its index + * @brief Select the plan to execute using its index. * @param[in] index Index of the plan to execute, can be obtained via * getPlanNameList() * @return True: success, false: failed + * @version C++, Python */ bool executePlanByIndex(int32_t index); /** - * @brief Select the plan to execute using its name + * @brief Select the plan to execute using its name. * @param[in] name Name of the plan to execute, can be obtained via * getPlanNameList() * @return True: success, false: failed + * @version C++, Python */ bool executePlanByName(const std::string& name); /** - * @brief Get a list of all available plans - * @param[out] planNameList String list of available plans - * @return True: success, false: failed + * @brief Get a list of all available plans. + * @return String list of available plans + * @version C++, Python */ - bool getPlanNameList(std::vector* planNameList); + std::vector getPlanNameList(void) const; /** - * @brief Get detailed information about the currently running plan + * @brief Get detailed information about the currently running plan. * @param[out] planInfo PlanInfo struct * @return True: success, false: failed * @note Contains stuff like plan name, primitive name, node name, node * path, node path time period, etc. + * @version C++, Python */ bool getPlanInfo(PlanInfo* planInfo); /** - * @brief Select a primitive to execute using its name and parameters + * @brief Select a primitive to execute using its name and parameters. * @param[in] ptCmd Primitive command with the format: * ptName(inputParam1=xxx, inputParam2=xxx, ...) * @return True: success, false: failed + * @version C++, Python */ bool executePrimitive(const std::string& ptCmd); /** * @brief If the mounted tool has more than one TCP, switch the TCP being - * used by the robot server. Default to the 1st one (index = 0) + * used by the robot server. Default to the 1st one (index = 0). * @param[in] index Index of the TCP on the mounted tool to switch to * @return True: success, false: failed * @note No need to call this method if the mounted tool on the robot has * only one TCP, it'll be used by default + * @version C++, Python */ bool switchTcp(int32_t index); /** * @brief Start auto recovery to bring joints that are outside the allowed - * position range back into allowed range + * position range back into allowed range. * @return True: success, false: failed * @note Refer to isRecoveryState() and user manual for more details. + * @version C++, Python */ bool startAutoRecovery(void); //=========================== MOTION METHODS =============================== /** - * @brief [Realtime] Continuously send joint torque command to robot + * @brief [Real-time] Continuously send joint torque command to robot. * @param[in] torques \f$ \mathbb{R}^{Dof \times 1} \f$ target torques of - * all joints, \f$ {\tau_J}_d~[Nm] \f$ + * the joints, \f$ {\tau_J}_d~[Nm] \f$ * @param[in] enableGravityComp Enable/disable robot gravity compensation * @param[in] enableJointSoftLimits Enable/disable soft limits to keep the * joints from moving outside the allowed position range, which will @@ -239,31 +263,59 @@ class Robot * @note Set mode to MODE_JOINT_TORQUE, see flexiv::Mode * @warning Always send smooth and continuous commands to avoid sudden * movements + * @version C++ */ bool streamJointTorque(const std::vector& torques, bool enableGravityComp = true, bool enableJointSoftLimits = true); /** - * @brief [Realtime] Continuously send joint position, velocity and - * acceleration command + * @brief [Real-time] Continuously send joint position, velocity and + * acceleration command. * @param[in] positions \f$ \mathbb{R}^{Dof \times 1} \f$ target positions - * of all joints, \f$ q_d~[rad] \f$ + * of the joints, \f$ q_d~[rad] \f$ * @param[in] velocities \f$ \mathbb{R}^{Dof \times 1} \f$ target velocities - * of all joints, \f$ \dot{q}_d~[rad/s] \f$ + * of the joints, \f$ \dot{q}_d~[rad/s] \f$ * @param[in] accelerations \f$ \mathbb{R}^{Dof \times 1} \f$ target - * accelerations of all joints, \f$ \ddot{q}_d~[rad/s^2] \f$ + * accelerations of the joints, \f$ \ddot{q}_d~[rad/s^2] \f$ * @return True: success, false: failed * @note Set mode to MODE_JOINT_POSITION, see flexiv::Mode * @warning Always send smooth and continuous commands to avoid sudden * movements + * @version C++ */ bool streamJointPosition(const std::vector& positions, const std::vector& velocities, const std::vector& accelerations); /** - * @brief [Realtime] Continuously send TCP pose, velocity and acceleration - * command + * @brief [Non-real-time] Discretely send joint position, velocity and + * acceleration command. The internal trajectory generator will interpolate + * between two set points and make the motion smooth. + * @param[in] positions \f$ \mathbb{R}^{Dof \times 1} \f$ target positions + * of the joints, \f$ q_d~[rad] \f$ + * @param[in] velocities \f$ \mathbb{R}^{Dof \times 1} \f$ target velocities + * of the joints, \f$ \dot{q}_d~[rad/s] \f$ + * @param[in] accelerations \f$ \mathbb{R}^{Dof \times 1} \f$ target + * accelerations of the joints, \f$ \ddot{q}_d~[rad/s^2] \f$ + * @param[in] maxVel \f$ \mathbb{R}^{Dof \times 1} \f$ maximum velocities + * of the joints, \f$ \dot{q}_max~[rad/s] \f$ + * @param[in] maxAcc \f$ \mathbb{R}^{Dof \times 1} \f$ maximum accelerations + * of the joints, \f$ \ddot{q}_max~[rad/s^2] \f$ + * @param[in] maxJerk \f$ \mathbb{R}^{Dof \times 1} \f$ maximum jerk + * of the joints, \f$ \dddot{q}_max~[rad/s^3] \f$ + * @return True: success, false: failed + * @note Set mode to MODE_JOINT_POSITION_NRT, see flexiv::Mode + * @version C++, Python + */ + bool sendJointPosition(const std::vector& positions, + const std::vector& velocities, + const std::vector& accelerations, + const std::vector& maxVel, const std::vector& maxAcc, + const std::vector& maxJerk); + + /** + * @brief [Real-time] Continuously send TCP pose, velocity and acceleration + * command. * @param[in] pose \f$ \mathbb{R}^{7 \times 1} \f$ target TCP pose * in base frame, \f$ \mathbb{R}^{3 \times 1} \f$ position and \f$ * \mathbb{R}^{4 \times 1} \f$ quaternion \f$ [x, y, z, q_w, q_x, q_y, @@ -277,35 +329,57 @@ class Robot * acceleration and \f$ \mathbb{R}^{3 \times 1} \f$ angular acceleration \f$ * [a_x, a_y, a_z, \alpha_x, \alpha_y, \alpha_z]^T \f$ * @return True: success, false: failed - * @note Set mode to flexiv::Mode::MODE_CARTESIAN_IMPEDANCE, see - * flexiv::Mode + * @note Set mode to MODE_CARTESIAN_IMPEDANCE, see flexiv::Mode * @warning Always send smooth and continuous commands to avoid sudden * movements + * @version C++ */ bool streamTcpPose(const std::vector& pose, const std::vector& velocity, const std::vector& acceleration); /** - * @brief Check if the robot has come to a complete stop + * @brief [Non-real-time] Discretely send TCP pose command with contact + * force constraints. The internal trajectory generator will interpolate + * between two set points and make the motion smooth. + * @param[in] pose \f$ \mathbb{R}^{7 \times 1} \f$ target TCP pose + * in base frame, \f$ \mathbb{R}^{3 \times 1} \f$ position and \f$ + * \mathbb{R}^{4 \times 1} \f$ quaternion \f$ [x, y, z, q_w, q_x, q_y, + * q_z]^T \f$ + * @param[in] maxWrench \f$ \mathbb{R}^{6 \times 1} \f$ maximum contact + * wrench in TCP coordinate, \f$ \mathbb{R}^{3 \times 1} \f$ force and \f$ + * \mathbb{R}^{3 \times 1} \f$ moment \f$ [f_x, f_y, f_z, m_x, m_y, m_z]^T + * \f$ + * @return True: success, false: failed + * @note Set mode to MODE_CARTESIAN_IMPEDANCE_NRT, see flexiv::Mode + * @version C++, Python + */ + bool sendTcpPose( + const std::vector& pose, const std::vector& maxWrench); + + /** + * @brief Check if the robot has come to a complete stop. * @return True: stopped, false: still moving + * @version C++, Python */ bool isStopped(void) const; //============================= IO METHODS ================================= /** - * @brief Set digital output on the control box + * @brief Set digital output on the control box. * @param[in] portNumber Port to set value to [0 ~ 15] * @param[in] value True: set high, false: set low * @return True: success, false: failed + * @version C++, Python */ bool writeDigitalOutput(int32_t portNumber, bool value); /** - * @brief Read digital input on the control box + * @brief Read digital input on the control box. * @param[in] portNumber Port to read value from [0 ~ 15] * @param[out] value True: port high, false: port low * @return True: success, false: failed + * @version C++, Python */ bool readDigitalInput(int32_t portNumber, bool* value); diff --git a/include/RobotStates.hpp b/include/RobotStates.hpp index 9826ced8..44722128 100644 --- a/include/RobotStates.hpp +++ b/include/RobotStates.hpp @@ -187,7 +187,7 @@ struct SystemStatus /** * @brief If joint limit is triggered */ - bool m_jntLimitTriggered; + bool m_jointLimitTriggered; /** * @brief Primitive states information diff --git a/lib/flexivrdk.so b/lib/flexivrdk.so new file mode 100755 index 00000000..206c9065 Binary files /dev/null and b/lib/flexivrdk.so differ diff --git a/lib/libFlexivRdk.a b/lib/libFlexivRdk.a index e066e5c5..600d80ac 100644 Binary files a/lib/libFlexivRdk.a and b/lib/libFlexivRdk.a differ diff --git a/test/test_data_integrity.cpp b/test/test_data_integrity.cpp index 598adf82..ba50446d 100644 --- a/test/test_data_integrity.cpp +++ b/test/test_data_integrity.cpp @@ -1,7 +1,7 @@ /** * @test test_scheduler.cpp * A test to evaluate RDK's internal real-time scheduler. - * @copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. + * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ diff --git a/test/test_dynamic_engine.cpp b/test/test_dynamic_engine.cpp index a64126a9..e62cac5e 100644 --- a/test/test_dynamic_engine.cpp +++ b/test/test_dynamic_engine.cpp @@ -1,7 +1,7 @@ /** * @test test_dynamic_engine.cpp * A test to evaluate the dynamics engine (J, M, G) - * @copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. + * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ diff --git a/test/test_endurance.cpp b/test/test_endurance.cpp index 26a85ae5..7d83212f 100644 --- a/test/test_endurance.cpp +++ b/test/test_endurance.cpp @@ -3,7 +3,7 @@ * Endurance test running Cartesian impedance control to slowly sine-sweep near * home for a duration of user-specified hours. Raw data will be logged to CSV * files continuously. - * @copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. + * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ diff --git a/test/test_log.cpp b/test/test_log.cpp index a465ef1b..55be86d3 100644 --- a/test/test_log.cpp +++ b/test/test_log.cpp @@ -1,7 +1,7 @@ /** * @test test_log.cpp * Test log functions of the flexiv::Log class - * @copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. + * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ diff --git a/test/test_loop_latency.cpp b/test/test_loop_latency.cpp index 9a2aaa47..3fc97e89 100644 --- a/test/test_loop_latency.cpp +++ b/test/test_loop_latency.cpp @@ -3,7 +3,7 @@ * A test to benchmark RDK's loop latency, including communication, computation, * etc. The workstation PC's serial port is used as reference, and the robot * server's digital out port is used as test target. - * @copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. + * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ diff --git a/test/test_scheduler.cpp b/test/test_scheduler.cpp index 10c36756..0a33baf0 100644 --- a/test/test_scheduler.cpp +++ b/test/test_scheduler.cpp @@ -1,7 +1,7 @@ /** * @test test_scheduler.cpp * A test to evaluate RDK's internal real-time scheduler. - * @copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. + * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ diff --git a/test/test_set_tool.cpp b/test/test_set_tool.cpp index 70dc2b44..d69da792 100644 --- a/test/test_set_tool.cpp +++ b/test/test_set_tool.cpp @@ -1,7 +1,7 @@ /** * @test test_set_tool.cpp * A test to evaluate the setTool API. - * @copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. + * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. * @author Flexiv */ diff --git a/test/test_timeliness_monitor.cpp b/test/test_timeliness_monitor.cpp index e29c979b..eec655cf 100644 --- a/test/test_timeliness_monitor.cpp +++ b/test/test_timeliness_monitor.cpp @@ -6,7 +6,7 @@ * first, then if the check has failed too many times, the RDK connection with * the server will be closed. During this test, the robot will hold its position * using joint torque streaming mode. - * @copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. + * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved. * @author Flexiv */