diff --git a/.github/helpers/check_urls.sh b/.github/helpers/check_urls.sh index 5d3662ee..d3aa1861 100755 --- a/.github/helpers/check_urls.sh +++ b/.github/helpers/check_urls.sh @@ -38,6 +38,7 @@ urls=$(grep -oP '(http|ftp|https):\/\/([a-zA-Z0-9_-]+(?:(?:\.[a-zA-Z0-9_-]+)+))( fail_counter=0 FAILED_LINKS=() +CHECKED_LINKS=() for item in $urls; do # echo $item skip=0 @@ -53,15 +54,18 @@ for item in $urls; do filename=$(echo "$item" | cut -d':' -f1) url=$(echo "$item" | cut -d':' -f2-) echo -n "Checking $url from file $filename" - if ! curl --head --silent --fail "$url" 2>&1 > /dev/null; then - echo -e " \033[0;31mNOT FOUND\033[32m\n" + if [[ $(echo ${CHECKED_LINKS[@]} | fgrep -w $url) ]]; then + echo -e " \033[36malready checked\033[0m" + elif ! curl --head --silent --fail "$url" 2>&1 > /dev/null; then + echo -e " \033[0;31mNOT FOUND\033[0m" FAILED_LINKS+=("$url from file $filename") ((fail_counter=fail_counter+1)) else - printf " \033[32mok\033[0m\n" + echo -e " \033[32mok\033[0m" fi + CHECKED_LINKS+=("$url") done echo "Failed files:" printf '%s\n' "${FAILED_LINKS[@]}" -exit $fail_counter \ No newline at end of file +exit $fail_counter diff --git a/doc/architecture/instruction_executor.rst b/doc/architecture/instruction_executor.rst index 2e2f9963..65818737 100644 --- a/doc/architecture/instruction_executor.rst +++ b/doc/architecture/instruction_executor.rst @@ -1,3 +1,5 @@ +.. _instruction_executor: + Instruction Executor ==================== @@ -21,12 +23,4 @@ for sending motion instructions to the robot. Hence, it requires a :ref:`ur_driv Therefore, all parameters and restrictions of these functions apply. For example, velocity and acceleration parameters will be ignored if there is a time > 0 given. -As a minimal working example, please see ``examples/instruction_executor.cpp`` example: - -.. literalinclude:: ../../examples/instruction_executor.cpp - :language: c++ - :caption: examples/instruction_executor.cpp - :linenos: - :lineno-match: - :start-at: g_my_driver.reset - :end-at: g_my_driver->stopControl(); +As a minimal working example, please see the :ref:`instruction_executor_example`. diff --git a/doc/architecture/script_sender.rst b/doc/architecture/script_sender.rst index 114f4f12..4644c186 100644 --- a/doc/architecture/script_sender.rst +++ b/doc/architecture/script_sender.rst @@ -13,15 +13,7 @@ corresponding request when starting a program on the robot that contains the **E program node. In order to work properly, make sure that the IP address and script sender port are configured correctly on the robot. -The following example creates a ``ScriptSender`` listening on port ``12345`` and sends the script -``textmsg("Hello, World!")`` when requested. A fully compilable example can be found in `script_sender.cpp `_ - -.. literalinclude:: ../../examples/script_sender.cpp - :language: c++ - :caption: examples/script_sender.cpp - :linenos: - :lineno-match: - :start-at: constexpr uint32_t PORT +An example of how to use the ``ScriptSender`` class can be found in the :ref:`script_sender_example`. .. note:: PolyScope X users cannot use the URCap linked above. There is a development version of a URCapX diff --git a/doc/example.rst b/doc/example.rst deleted file mode 100644 index da167bf9..00000000 --- a/doc/example.rst +++ /dev/null @@ -1,12 +0,0 @@ -.. _example-driver: - -Example driver -============== -In the ``examples`` subfolder you will find a minimal example of a running driver. It starts an -instance of the ``UrDriver`` class and prints the RTDE values read from the controller. To run it make -sure to - -* have an instance of a robot controller / URSim running at the configured IP address (or adapt the - address to your needs) -* run it from the package's main folder (the one where this README.md file is stored), as for - simplicity reasons it doesn't use any sophisticated method to locate the required files. diff --git a/doc/examples.rst b/doc/examples.rst new file mode 100644 index 00000000..4e5ad519 --- /dev/null +++ b/doc/examples.rst @@ -0,0 +1,28 @@ +Usage examples +============== + +This library contains a number of examples how this library can be used. You can use them as a +starting point for your own applications. + +All examples take a robot's IP address as a first argument and some of them take a maximum run +duration (in seconds) as second argument. If the second argument is omitted, some of the examples +may be running forever until manually stopped. + +.. note:: Most of these examples use the driver's headless mode. Therefore, on an e-Series (PolyScope 5) robot, the robot has to be + in *remote control mode* to work. + +.. toctree:: + :maxdepth: 1 + + examples/dashboard_client + examples/force_mode + examples/freedrive + examples/instruction_executor + examples/primary_pipeline + examples/primary_pipeline_calibration + examples/rtde_client + examples/script_sender + examples/spline_example + examples/tool_contact_example + examples/trajectory_point_interface + examples/ur_driver diff --git a/doc/examples/dashboard_client.rst b/doc/examples/dashboard_client.rst new file mode 100644 index 00000000..2b5ae406 --- /dev/null +++ b/doc/examples/dashboard_client.rst @@ -0,0 +1,82 @@ +:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/dashboard_client.rst + +Dashboard client example +======================== + +This example shows how to use the builtin `Dashboard server `_ to communicate with a robot. + +.. note:: + + The Dashboard Server is only available on CB3 and e-Series robots. It is not available on + PolyScope X. + + +The `dashboard_example.cpp `_ shows how to use this class: + +.. note:: For the following example to work on an e-Series (PolyScope 5) robot, the robot has to be + in *remote control mode*. + +.. literalinclude:: ../../examples/dashboard_example.cpp + :language: c++ + :caption: examples/dashboard_example.cpp + :linenos: + :lineno-match: + :start-at: std::make_unique + :end-at: my_dashboard->commandCloseSafetyPopup(); + +At first, a ``DashboardClient`` object is created with the IP address of the robot. Then, the +client is connected to the robot. After that, the client sends a command to the robot and receives +the answer. + +Some commands support getting the response from the robot. For example, the +``commandPolyScopeVersion()`` function: + +.. literalinclude:: ../../examples/dashboard_example.cpp + :language: c++ + :caption: examples/dashboard_example.cpp + :linenos: + :lineno-match: + :start-at: // Get the PolyScope version + :end-at: URCL_LOG_INFO(version.c_str()); + + +The ``DashboardClient`` can easily be used to cycle through the robot's states, for example for +initialization: + +.. literalinclude:: ../../examples/dashboard_example.cpp + :language: c++ + :caption: examples/dashboard_example.cpp + :linenos: + :lineno-match: + :start-at: // Power it on + :end-at: // Load existing program + +All commands are blocking and will wait for the necessary action being done. The dashboard server's +response will be compared with an expected response. For example, when calling +``commandPowerOn(timeout)``, it is checked that the dashboard server is answering ``"Powering on"`` and +then it is queried until the robot reports ``"Robotmode: IDLE"`` or until the timeout is reached. +The example contains more commands that follow the same scheme. + + +If you want to send a query / command to the dashboard server and only want to receive the +response, you can use the ``sendAndReceive()`` function: + +.. literalinclude:: ../../examples/dashboard_example.cpp + :language: c++ + :caption: examples/dashboard_example.cpp + :linenos: + :lineno-match: + :start-at: // Make a raw request and save the response + :end-at: URCL_LOG_INFO("Program state: %s", program_state.c_str()); + +For checking the response against an expected regular expression use ``sendRequest()``: + +.. literalinclude:: ../../examples/dashboard_example.cpp + :language: c++ + :caption: examples/dashboard_example.cpp + :linenos: + :lineno-match: + :start-at: // The response can be checked with a regular expression + :end-at: URCL_LOG_INFO("Power off command success: %d", success); + + diff --git a/doc/examples/force_mode.rst b/doc/examples/force_mode.rst new file mode 100644 index 00000000..56ab6324 --- /dev/null +++ b/doc/examples/force_mode.rst @@ -0,0 +1,63 @@ +:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/force_mode.rst + +Force Mode example +================== + +The ``ur_client_library`` supports leveraging the robot's force mode directly. An example on how to +use it can be found in `force_mode_example.cpp `_. + +In order to utilize force mode, we'll have to create and initialize a full ``UrDriver`` object +first: + +.. literalinclude:: ../../examples/force_mode_example.cpp + :language: c++ + :caption: examples/force_mode_example.cpp + :linenos: + :lineno-match: + :start-at: // Now the robot is ready to receive a program + :end-at: // End of initialization + +Start force mode +---------------- + +After that, we can start force mode by calling the ``startForceMode()`` function: + +.. literalinclude:: ../../examples/force_mode_example.cpp + :language: c++ + :caption: examples/force_mode_example.cpp + :linenos: + :lineno-match: + :start-at: // Start force mode + :end-at: if (!success) + +All parameters for the force mode are included into the ``startForceMode()`` function call. If you +want to change the parameters, e.g. change the forces applied, you can simply call +``startForceMode()`` again with the new parameters. + +.. note:: + CB3 robots don't support specifying force_mode's ``gain_scaling``, so there are two different + functions available. + +Once force mode is started successfully, we'll have to send keepalive messages to the robot in +order to keep the communication active: + +.. literalinclude:: ../../examples/force_mode_example.cpp + :language: c++ + :caption: examples/force_mode_example.cpp + :linenos: + :lineno-match: + :start-at: std::chrono::duration time_done(0); + :end-at: URCL_LOG_INFO("Timeout reached."); + +Stop force mode +--------------- + +Once finished, force_mode can be stopped by calling ``endForceMode()``. + +.. literalinclude:: ../../examples/force_mode_example.cpp + :language: c++ + :caption: examples/force_mode_example.cpp + :linenos: + :lineno-match: + :start-at: endForceMode() + :end-at: endForceMode() diff --git a/doc/examples/freedrive.rst b/doc/examples/freedrive.rst new file mode 100644 index 00000000..9b3d9fd3 --- /dev/null +++ b/doc/examples/freedrive.rst @@ -0,0 +1,70 @@ +:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/freedrive.rst + +Freedrive Mode example +====================== + +`Freedrive +`_ +allows the robot arm to be manually pulled into desired positions and/or poses. The joints move +with little resistance because the brakes are released. + +An example to utilize the freedrive mode can be found in the `freedrive_example.cpp `_. + +.. note:: For the following example to work on an e-Series (PolyScope 5) robot, the robot has to be + in *remote control mode*. + +At first, we create a ``ExampleRobotWrapper`` object in order to initialize communication with the +robot. + +.. literalinclude:: ../../examples/freedrive_example.cpp + :language: c++ + :caption: examples/freedrive_example.cpp + :linenos: + :lineno-match: + :start-at: bool headless_mode = true; + :end-before: URCL_LOG_INFO("Starting freedrive mode"); + + +Start freedrive mode +-------------------- + +The ``UrDriver`` provides a method to start freedrive mode directly: + +.. literalinclude:: ../../examples/freedrive_example.cpp + :language: c++ + :caption: examples/freedrive_example.cpp + :linenos: + :lineno-match: + :start-at: URCL_LOG_INFO("Starting freedrive mode"); + :end-at: sendFreedriveMessageOrDie(control::FreedriveControlMessage::FREEDRIVE_START); + +As it is potentially dangerous to leave the robot in freedrive mode, the robot program expect +frequent keepalive messages to verify that the remote connection is still available and freedrive +mode is being expected to be active. + +Freedrive mode will be active from this point on until it is either stopped, or no keepalive +message is received by the robot anymore. + +Therefore, we have to make sure to send regular keepalive messages to the robot. The following +section will keep freedrive mode active for a period of time defined in ``seconds_to_run``. + +.. literalinclude:: ../../examples/freedrive_example.cpp + :language: c++ + :caption: examples/freedrive_example.cpp + :linenos: + :lineno-match: + :start-at: std::chrono::duration time_done(0); + :end-before: sendFreedriveMessageOrDie(control::FreedriveControlMessage::FREEDRIVE_STOP); + +Stop force Mode +--------------- + +To stop force mode either stop sending keepalive signals or request deactivating it explicitly: + +.. literalinclude:: ../../examples/freedrive_example.cpp + :language: c++ + :caption: examples/freedrive_example.cpp + :linenos: + :lineno-match: + :start-at: sendFreedriveMessageOrDie(control::FreedriveControlMessage::FREEDRIVE_STOP); + :end-at: sendFreedriveMessageOrDie(control::FreedriveControlMessage::FREEDRIVE_STOP); diff --git a/doc/examples/instruction_executor.rst b/doc/examples/instruction_executor.rst new file mode 100644 index 00000000..f4e21ec0 --- /dev/null +++ b/doc/examples/instruction_executor.rst @@ -0,0 +1,66 @@ +:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/instruction_executor.rst + +.. _instruction_executor_example: + +Instruction Executor example +============================ + +This example shows how to use the :ref:`instruction_executor` class. It can be used for easily +executing a sequence of instructions such as motions on the robot using the built-in URScript functions. + +The `instruction_executor.cpp `_ shows how to use this class: + +.. note:: For the instruciton executor to work there has to be an established connection to the + :ref:`reverse_interface`. That means, the respective program has to be running on the robot. The + example below will do that automatically, if the connected robot is in *remote_control* mode. + + +.. literalinclude:: ../../examples/instruction_executor.cpp + :language: c++ + :caption: examples/instruction_executor.cpp + :linenos: + :lineno-match: + :start-at: std::unique_ptr tool_comm_setup; + :end-at: auto instruction_executor = std::make_shared(g_my_driver); + +At first, a ``InstructionExecutor`` object is created with the URDriver object as it needs that +for communication with the robot. + +Currently, the ``InstructionExecutor`` can either be used to run sequences of motions or single motions. + +Execute a sequence of motions +----------------------------- + +To run a sequence of motions, create an +``std::vector>`` and pass it to the +``executeMotion`` function: + +.. literalinclude:: ../../examples/instruction_executor.cpp + :language: c++ + :caption: examples/instruction_executor.cpp + :linenos: + :lineno-match: + :start-at: // Trajectory definition + :end-at: instruction_executor->executeMotion(motion_sequence); + +Each element in the motion sequence can be a different motion type. In the example, there are two +``MoveJ`` motions and two ``MoveL`` motion. The primitives' parameters are directly forwarded to +the underlying script functions, so the parameter descriptions for them apply, as well. +Particularly, you may want to choose between either a time-based execution speed or an acceleration +/ velocity parametrization. The latter will be ignored if a time > 0 is given. + +Execute a single motion +----------------------- + +To run a single motion, the ``InstructionExecutor`` provides the methods ``moveJ(...)`` and +``moveL(...)``: + +.. literalinclude:: ../../examples/instruction_executor.cpp + :language: c++ + :caption: examples/instruction_executor.cpp + :linenos: + :lineno-match: + :start-at: double goal_time_sec = 2.0; + :end-before: g_my_driver->stopControl(); + +Again, time parametrization has priority over acceleration / velocity parameters. diff --git a/doc/examples/primary_pipeline.rst b/doc/examples/primary_pipeline.rst new file mode 100644 index 00000000..24b62f42 --- /dev/null +++ b/doc/examples/primary_pipeline.rst @@ -0,0 +1,78 @@ +:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/primary_pipeline.rst + +.. _primary_pipeline_example: + +Primary Pipeline example +======================== + +This example shows how to use the ``Pipeline`` class for the robot's `primary interface +`_. +It reads all packages coming in from the robot't primary interface and prints their contents. + +At the current time parsing primary interface data is very limited, so this example will print the +raw binary data for most package types. The example serves to demonstrate the basic control flow +used for reading data from the robot. + +In this library, a "pipeline" uses a producer / consumer architecture. A producer is reading data +from a *stream*, parses that data and puts it into a *queue*. A consumer reads data from the queue +and can do whatever its purpose is. + +Producer setup +-------------- + +To setup the producer, we need to create a stream, a parser and create a producer with those: + +.. literalinclude:: ../../examples/primary_pipeline.cpp + :language: c++ + :caption: examples/primary_pipeline.cpp + :linenos: + :lineno-match: + :start-at: // First of all, we need a stream + :end-at: prod.setupProducer(); + +Consumer setup +-------------- + +The consumer can be any consumer that is able to consume data produced by the producer, in this +case ``urcl::primary_interface::PrimaryPackage``. Here, we use a ``ShellExecutor`` that will try to +print each package's content to the shell output: + +.. literalinclude:: ../../examples/primary_pipeline.cpp + :language: c++ + :caption: examples/primary_pipeline.cpp + :linenos: + :lineno-match: + :start-at: // The shell consumer + :end-at: auto consumer = std::make_unique + +Assemble the pipeline +--------------------- + +Finally, we need to assemble the pipeline by connecting the producer to the consumer: + +.. literalinclude:: ../../examples/primary_pipeline.cpp + :language: c++ + :caption: examples/primary_pipeline.cpp + :linenos: + :lineno-match: + :start-after: auto consumer = std::make_unique + :end-at: pipeline.run() + +You can setup a custom notifier that can handle start and stop events of the pipeline. In this +example we use the basic ``INotifier`` which doesn't do anything. + +With all that, we can create the ``pipeline`` by passing the producer, consumer, a name and the +notifier to it's constructor. + +From this point on, the producer will read the data coming on from the stream and that data will be +processed by the consumer. We keep the example program alive for a while to see some data: + +.. literalinclude:: ../../examples/primary_pipeline.cpp + :language: c++ + :caption: examples/primary_pipeline.cpp + :linenos: + :lineno-match: + :start-at: do + :end-at: return 0 + + diff --git a/doc/examples/primary_pipeline_calibration.rst b/doc/examples/primary_pipeline_calibration.rst new file mode 100644 index 00000000..fa9d7845 --- /dev/null +++ b/doc/examples/primary_pipeline_calibration.rst @@ -0,0 +1,52 @@ +:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/primary_pipeline_calibration.rst + +.. _primary_pipeline_calibration_example: + +Primary Pipeline Calibration example +==================================== + +This example is very similar to the :ref:`primary_pipeline_example`. However, it uses a +specialized consumer that will analyze the calibration data sent from the robot instead of the +``ShellConsumer``. + +Consumer setup +-------------- + +This example uses a specialized type of consumer that stores data about the received robot +calibration. + +.. literalinclude:: ../../examples/primary_pipeline_calibration.cpp + :language: c++ + :caption: examples/primary_pipeline_calibration.cpp + :linenos: + :lineno-match: + :start-at: class CalibrationConsumer + :end-at: }; + +Since the producer is reading every package from the primary interface, the consumer has to be able +to consume any primary package. + +Assemble the pipeline +--------------------- + +The rest of the pipeline setup is the same as in the other pipeline example, just that we create a +``CalibrationConsumer`` instead of a ``ShellConsumer``: + +.. literalinclude:: ../../examples/primary_pipeline_calibration.cpp + :language: c++ + :caption: examples/primary_pipeline_calibration.cpp + :linenos: + :lineno-match: + :start-at: // The calibration consumer + :end-at: calib_pipeline.run() + +Again, product handling will be happening in the background, so in the rest of the program we wait +until the calibration consumer received and processed data and then print information about that: + +.. literalinclude:: ../../examples/primary_pipeline_calibration.cpp + :language: c++ + :caption: examples/primary_pipeline_calibration.cpp + :linenos: + :lineno-match: + :start-at: while (!calib_consumer + :end-at: return 0 diff --git a/doc/examples/rtde_client.rst b/doc/examples/rtde_client.rst new file mode 100644 index 00000000..bf47d97b --- /dev/null +++ b/doc/examples/rtde_client.rst @@ -0,0 +1,89 @@ +:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/rtde_client.rst + +.. _rtde_client_example: + +RTDE Client example +=================== + +This example shows how to use the ``RTDEClient`` class for the robot's `Real-Time Data Exchange +(RTDE) interface +`_. + +The RTDE client has to be initialized with a list of keys that should be streamed from the robot +and a list of keys that should be sent to the robot. The client will then start a background thread +establishing communication. + +In this example, those keys are stored in two text files relative to this repository's root: +``rtde_input_keys.txt`` and ``rtde_output_keys.txt``. The example will read those files and use them +to initialize the RTDE client. + +.. literalinclude:: ../../examples/rtde_client.cpp + :language: c++ + :caption: examples/rtde_client.cpp + :linenos: + :lineno-match: + :start-at: const std::string OUTPUT_RECIPE + :end-at: const std::string INPUT_RECIPE + + +Internally, the RTDE client uses the same producer / consumer architecture as show in the +:ref:`primary_pipeline_example` example. However, it doesn't have a consumer thread, so data has to +be read by the user to avoid the pipeline's queue from overflowing. + +Creating an RTDE Client +----------------------- + +An RTDE client can be directly created passing the robot's IP address, a ``INotifier`` object, an +output and an input recipe. Optionally, a communication frequency can be passed as well. If that is +omitted, RTDE communication will be established at the robot's control frequency. + +.. literalinclude:: ../../examples/rtde_client.cpp + :language: c++ + :caption: examples/rtde_client.cpp + :linenos: + :lineno-match: + :start-at: comm::INotifier notifier; + :end-at: my_client.init(); + +An RTDE data package containing every key-value pair from the output recipe can be fetched using +the ``getDataPackage()`` method. This method will block until a new package is available. + + +Reading data from the RTDE client +--------------------------------- + +Once the RTDE client is initialized, we'll have to start communication separately. As mentioned +above, we'll have to read data from the client once communication is started, hence we start +communication right before a loop reading data. + +.. literalinclude:: ../../examples/rtde_client.cpp + :language: c++ + :caption: examples/rtde_client.cpp + :linenos: + :lineno-match: + :start-at: // Once RTDE communication is started + :end-before: // Change the speed slider + +Writing Data to the RTDE client +------------------------------- + +In this example, we use the RTDE client to oscillate the speed slider on the teach pendant between +0 and 1. While this doesn't bear any practical use it shows how sending data to the RTDE interface +works. + +To send data to the RTDE client, we can use ``RTDEWriter`` object stored in the RTDE client. This +has methods implemented for each data type that can be sent to the robot. The input recipe used to +initialize the RTDE client has to contain the keys necessary to send that specific data. + +.. literalinclude:: ../../examples/rtde_client.cpp + :language: c++ + :caption: examples/rtde_client.cpp + :linenos: + :lineno-match: + :start-at: my_client.getWriter().sendSpeedSlider + :end-at: } + + +.. note:: Many RTDE inputs require setting up the data key and a mask key. See the `RTDE guide + `_ + for more information. diff --git a/doc/examples/script_sender.rst b/doc/examples/script_sender.rst new file mode 100644 index 00000000..ba78d534 --- /dev/null +++ b/doc/examples/script_sender.rst @@ -0,0 +1,16 @@ +:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/script_sender.rst + +.. _script_sender_example: + +Script Sender example +===================== + +The following example creates a ``ScriptSender`` listening on port ``12345`` and sends the script +``textmsg("Hello, World!")`` when requested. A fully compilable example can be found in `script_sender.cpp `_ + +.. literalinclude:: ../../examples/script_sender.cpp + :language: c++ + :caption: examples/script_sender.cpp + :linenos: + :lineno-match: + :start-at: constexpr uint32_t PORT diff --git a/doc/examples/spline_example.rst b/doc/examples/spline_example.rst new file mode 100644 index 00000000..5faf50e3 --- /dev/null +++ b/doc/examples/spline_example.rst @@ -0,0 +1,14 @@ +:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/spline_example.rst + +.. _spline_example: + +Spline Interpolation example +============================ + +The URScript code inside `external_control.urscript +`_ implements trajectory execution based upon spline interpolation of a given sequence of waypoints. + +Depending on the waypoint parametrization, either cubic splines (positions, velocities, times) or +quintic splines (positions, velocities, accelerations, times) will be used. + +The example in `spline_example.cpp `_ shows how to use the :ref:`trajectory_point_interface` to execute trajectories using this spline interpolation. diff --git a/doc/examples/tool_contact_example.rst b/doc/examples/tool_contact_example.rst new file mode 100644 index 00000000..25cb9a95 --- /dev/null +++ b/doc/examples/tool_contact_example.rst @@ -0,0 +1,60 @@ +:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/tool_contact_example.rst + +.. _tool_contact_example: + +Tool Contact example +==================== + +Univeral Robots' **tool contact mode** allows detecting collisions between the robot's tool and the +environment and interrupting motions when that happens. This example demonstrates how to use the +tool contact mode to detect collisions and stop the robot. + +As a basic concept, we will move the robot linearly in the negative z axis until the tool hits +something or the program's timeout is hit. + +At first, we create a ``UrDriver`` object as usual: + +.. literalinclude:: ../../examples/tool_contact_example.cpp + :language: c++ + :caption: examples/tool_contact_example.cpp + :linenos: + :lineno-match: + :start-at: std::unique_ptr tool_comm_setup; + :end-before: g_my_driver->registerToolContactResultCallback + +We use a small helper function to make sure that the reverse interface is active and connected +before proceeding. + +When a tool contact is detected, a callback will be triggered. For that we define a function to +handle this event: + +.. literalinclude:: ../../examples/tool_contact_example.cpp + :language: c++ + :caption: examples/tool_contact_example.cpp + :linenos: + :lineno-match: + :start-at: void handleToolContactResult(control::ToolContactResult result) + :end-at: } + + + +This function is registered as a callback to the driver and then tool_contact mode is enabled: + +.. literalinclude:: ../../examples/tool_contact_example.cpp + :language: c++ + :caption: examples/tool_contact_example.cpp + :linenos: + :lineno-match: + :start-at: g_my_driver->registerToolContactResultCallback(&handleToolContactResult); + :end-at: g_my_driver->startToolContact() + +Once everything is initialized, we can start a control loop commanding the robot to move in the +negative z direction until the program timeout is hit or a tool contact is detected: + +.. literalinclude:: ../../examples/tool_contact_example.cpp + :language: c++ + :caption: examples/tool_contact_example.cpp + :linenos: + :lineno-match: + :start-at: // This will move the robot + :end-at: return 0; diff --git a/doc/examples/trajectory_point_interface.rst b/doc/examples/trajectory_point_interface.rst new file mode 100644 index 00000000..eb902b31 --- /dev/null +++ b/doc/examples/trajectory_point_interface.rst @@ -0,0 +1,103 @@ +:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/trajecotry_joint_interface_example.rst + +.. _trajecotry_joint_interface_example: + +Trajectory Joint Interface example +================================== + + + +At first, we create a ``UrDriver`` object as usual: + +.. literalinclude:: ../../examples/trajectory_point_interface.cpp + :language: c++ + :caption: examples/trajectory_point_interface.cpp + :linenos: + :lineno-match: + :start-at: std::unique_ptr tool_comm_setup; + :end-before: // --------------- INITIALIZATION END ------------------- + +We use a small helper function to make sure that the reverse interface is active and connected +before proceeding. + + +Initialization +-------------- + +As trajectory execution will be triggered asynchronously, we define a callback function to handle a +finished trajectory. A trajectory is considered finished when the robot is no longer executing it, +independent of whether it successfully reached its final point. The trajectory result will reflect +whether it was executed successfully, was canceled upon request or failed for some reason. + +.. literalinclude:: ../../examples/trajectory_point_interface.cpp + :language: c++ + :caption: examples/trajectory_point_interface.cpp + :linenos: + :lineno-match: + :start-at: void trajDoneCallback(const urcl::control::TrajectoryResult& result) + :end-at: } + +That callback can be registered at the ``UrDriver`` object: + + +.. literalinclude:: ../../examples/trajectory_point_interface.cpp + :language: c++ + :caption: examples/trajectory_point_interface.cpp + :linenos: + :lineno-match: + :start-at: g_my_driver->registerTrajectoryDoneCallback(&trajDoneCallback); + :end-at: g_my_driver->registerTrajectoryDoneCallback(&trajDoneCallback); + + +MoveJ Trajectory +---------------- + +Then, in order to execute a trajectory, we need to define a trajectory as a sequence of points and +parameters. The following example shows execution of a 2-point trajectory using URScript's +``movej`` function: + +.. literalinclude:: ../../examples/trajectory_point_interface.cpp + :language: c++ + :caption: examples/trajectory_point_interface.cpp + :linenos: + :lineno-match: + :start-after: // --------------- MOVEJ TRAJECTORY ------------------- + :end-before: // --------------- END MOVEJ TRAJECTORY ------------------- + +In fact, the path is followed twice, once parametrized by a segment duration and once using maximum +velocity / acceleration settings. If a duration > 0 is given for a segment, the velocity and +acceleration settings will be ignored as in the underlying URScript functions. In the example +above, each of the ``g_my_driver->writeTrajectoryPoint()`` calls will be translated into a +``movej`` command in URScript. + +While the trajectory is running, we need to tell the robot program that connection is still active +and we expect the trajectory to be running. This is being done by the +``g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP);`` +call. + +MoveL Trajectory +---------------- + +Similar to the ``movej``-based trajectory, execution can be done interpolating in joint space: + +.. literalinclude:: ../../examples/trajectory_point_interface.cpp + :language: c++ + :caption: examples/trajectory_point_interface.cpp + :linenos: + :lineno-match: + :start-after: // --------------- MOVEL TRAJECTORY ------------------- + :end-before: // --------------- END MOVEL TRAJECTORY ------------------- + +Spline based interpolation +-------------------------- + +Similar to the :ref:`spline_example`, the trajectory point interface can be used to execute motions +using the spline interpolation: + +.. literalinclude:: ../../examples/trajectory_point_interface.cpp + :language: c++ + :caption: examples/trajectory_point_interface.cpp + :linenos: + :lineno-match: + :start-after: // --------------- SPLINE TRAJECTORY ------------------- + :end-before: // --------------- END SPLINE TRAJECTORY ------------------- diff --git a/doc/examples/ur_driver.rst b/doc/examples/ur_driver.rst new file mode 100644 index 00000000..a146f198 --- /dev/null +++ b/doc/examples/ur_driver.rst @@ -0,0 +1,73 @@ +:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/ur_driver.rst + +.. _example-driver: + +Example driver +============== +In the ``examples`` subfolder you will find a minimal example of a running driver. It starts an +instance of the ``UrDriver`` class and prints the RTDE values read from the controller. To run it make +sure to + +* have an instance of a robot controller / URSim running at the configured IP address (or adapt the + address to your needs) +* run it from the package's main folder (the one where the README.md file is stored), as for + simplicity reasons it doesn't use any sophisticated method to locate the required files. + +This page will walk you through the `full_driver.cpp +`_ +example. + +Initialization +-------------- + +At first, we create a ``UrDriver`` object giving it the robot's IP address, script file and RTDE +recipes. + +.. literalinclude:: ../../examples/full_driver.cpp + :language: c++ + :caption: examples/full_driver.cpp + :linenos: + :lineno-match: + :start-at: std::unique_ptr tool_comm_setup; + :end-at: std::move(tool_comm_setup) + +Robot control loop +------------------ + +This example reads the robot's joint positions, increments / decrements the 5th axis and sends that +back as a joint command for the next cycle. This way, the robot will move its wrist first until a +positive limit and then back to 0. + +To read the joint data, the driver's RTDE client is used: + + +.. literalinclude:: ../../examples/full_driver.cpp + :language: c++ + :caption: examples/full_driver.cpp + :linenos: + :lineno-match: + :start-at: // Once RTDE communication is started + :end-before: // Open loop control + +The first read operation will initialize the target buffer with the current robot position. Next, +the target joint configuration is calculated: + + +.. literalinclude:: ../../examples/full_driver.cpp + :language: c++ + :caption: examples/full_driver.cpp + :linenos: + :lineno-match: + :start-at: // Open loop control + :end-at: joint_target[5] += increment + +To send the control command, the robot's :ref:`reverse_interface` is used via the +``writeJointCommand()`` function: + +.. literalinclude:: ../../examples/full_driver.cpp + :language: c++ + :caption: examples/full_driver.cpp + :linenos: + :lineno-match: + :start-at: // Setting the RobotReceiveTimeout + :end-before: URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg->toString().c_str()); diff --git a/doc/index.rst b/doc/index.rst index 7a2d6e12..0aa88d58 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -13,5 +13,5 @@ can be found on `GitHub commandPolyscopeVersion(version); + URCL_LOG_INFO(version.c_str()); + my_dashboard->commandCloseSafetyPopup(); // Power it on @@ -139,7 +144,13 @@ int main(int argc, char* argv[]) return 1; } - // Now the robot is ready to receive a program + // Make a raw request and save the response + std::string program_state = my_dashboard->sendAndReceive("programState"); + URCL_LOG_INFO("Program state: %s", program_state.c_str()); + + // The response can be checked with a regular expression + bool success = my_dashboard->sendRequest("power off", "Powering off"); + URCL_LOG_INFO("Power off command success: %d", success); return 0; } diff --git a/examples/force_mode_example.cpp b/examples/force_mode_example.cpp index 02bc9070..03faff5d 100644 --- a/examples/force_mode_example.cpp +++ b/examples/force_mode_example.cpp @@ -162,40 +162,35 @@ int main(int argc, char* argv[]) "for details."); } - // Once RTDE communication is started, we have to make sure to read from the interface buffer, as - // otherwise we will get pipeline overflows. Therefore, do this directly before starting your main - // loop. - g_my_driver->startRTDECommunication(); - - std::chrono::duration time_done(0); - std::chrono::duration timeout(second_to_run); - auto stopwatch_last = std::chrono::steady_clock::now(); - auto stopwatch_now = stopwatch_last; // Make sure that external control script is running if (!waitForProgramRunning()) { URCL_LOG_ERROR("External Control script not running."); return 1; } + // End of initialization -- We've started the external control program, which means we have to + // write keepalive signals from now on. Otherwise the connection will be dropped. + + // Start force mode // Task frame at the robot's base with limits being large enough to cover the whole workspace // Compliance in z axis and rotation around z axis bool success; if (g_my_driver->getVersion().major < 5) - success = g_my_driver->startForceMode({ 0, 0, 0, 0, 0, 0 }, // Task frame at the robot's base - { 0, 0, 1, 0, 0, 1 }, // Compliance in z axis and rotation around z axis - { 0, 0, 0, 0, 0, 0 }, // do not apply any active wrench - 2, // do not transform the force frame at all + success = g_my_driver->startForceMode({ 0, 0, 0, 0, 0, 0 }, // Task frame at the robot's base + { 0, 0, 1, 0, 0, 1 }, // Compliance in z axis and rotation around z axis + { 0, 0, -2, 0, 0, 0 }, // Press in -z direction + 2, // do not transform the force frame at all { 0.1, 0.1, 1.5, 3.14, 3.14, 0.5 }, // limits - 0.025); // damping_factor. See ScriptManual for details. + 0.005); // damping_factor. See ScriptManual for details. else { - success = g_my_driver->startForceMode({ 0, 0, 0, 0, 0, 0 }, // Task frame at the robot's base - { 0, 0, 1, 0, 0, 1 }, // Compliance in z axis and rotation around z axis - { 0, 0, 0, 0, 0, 0 }, // do not apply any active wrench - 2, // do not transform the force frame at all + success = g_my_driver->startForceMode({ 0, 0, 0, 0, 0, 0 }, // Task frame at the robot's base + { 0, 0, 1, 0, 0, 1 }, // Compliance in z axis and rotation around z axis + { 0, 0, -2, 0, 0, 0 }, // Press in -z direction + 2, // do not transform the force frame at all { 0.1, 0.1, 1.5, 3.14, 3.14, 0.5 }, // limits - 0.025, // damping_factor - 0.8); // gain_scaling. See ScriptManual for details. + 0.005, // damping_factor + 1.0); // gain_scaling. See ScriptManual for details. } if (!success) { @@ -203,31 +198,19 @@ int main(int argc, char* argv[]) return 1; } - while (true) + std::chrono::duration time_done(0); + std::chrono::duration timeout(second_to_run); + auto stopwatch_last = std::chrono::steady_clock::now(); + auto stopwatch_now = stopwatch_last; + while (time_done < timeout || second_to_run.count() == 0) { - // Read latest RTDE package. This will block for a hard-coded timeout (see UrDriver), so the - // robot will effectively be in charge of setting the frequency of this loop. - // In a real-world application this thread should be scheduled with real-time priority in order - // to ensure that this is called in time. - std::unique_ptr data_pkg = g_my_driver->getDataPackage(); - if (data_pkg) - { - g_my_driver->writeKeepalive(); - - if (time_done > timeout && second_to_run.count() != 0) - { - URCL_LOG_INFO("Timeout reached."); - break; - } - } - else - { - URCL_LOG_WARN("Could not get fresh data package from robot"); - } + g_my_driver->writeKeepalive(); stopwatch_now = std::chrono::steady_clock::now(); time_done += stopwatch_now - stopwatch_last; stopwatch_last = stopwatch_now; + std::this_thread::sleep_for(std::chrono::milliseconds(2)); } + URCL_LOG_INFO("Timeout reached."); g_my_driver->endForceMode(); } diff --git a/examples/freedrive_example.cpp b/examples/freedrive_example.cpp index e19a0d0b..48c0d8d7 100644 --- a/examples/freedrive_example.cpp +++ b/examples/freedrive_example.cpp @@ -58,7 +58,7 @@ void sendFreedriveMessageOrDie(const control::FreedriveControlMessage freedrive_ bool ret = g_my_robot->ur_driver_->writeFreedriveControlMessage(freedrive_action); if (!ret) { - URCL_LOG_ERROR("Could not send freedrive command. Is the robot in remote control?"); + URCL_LOG_ERROR("Could not send joint command. Is there an external_control program running on the robot?"); exit(1); } } @@ -106,6 +106,7 @@ int main(int argc, char* argv[]) stopwatch_now = std::chrono::steady_clock::now(); time_done += stopwatch_now - stopwatch_last; stopwatch_last = stopwatch_now; + std::this_thread::sleep_for(std::chrono::milliseconds(2)); } URCL_LOG_INFO("Stopping freedrive mode"); diff --git a/examples/full_driver.cpp b/examples/full_driver.cpp index e2cbe6d8..43b07725 100644 --- a/examples/full_driver.cpp +++ b/examples/full_driver.cpp @@ -43,17 +43,37 @@ const std::string DEFAULT_ROBOT_IP = "192.168.56.101"; const std::string SCRIPT_FILE = "resources/external_control.urscript"; const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt"; const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; -const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542"; std::unique_ptr g_my_driver; std::unique_ptr g_my_dashboard; vector6d_t g_joint_positions; +std::condition_variable g_program_running_cv; +std::mutex g_program_running_mutex; +bool g_program_running; + // We need a callback function to register. See UrDriver's parameters for details. void handleRobotProgramState(bool program_running) { // Print the text in green so we see it better std::cout << "\033[1;32mProgram running: " << std::boolalpha << program_running << "\033[0m\n" << std::endl; + if (program_running) + { + std::lock_guard lk(g_program_running_mutex); + g_program_running = program_running; + g_program_running_cv.notify_one(); + } +} + +bool waitForProgramRunning(int milliseconds = 100) +{ + std::unique_lock lk(g_program_running_mutex); + if (g_program_running_cv.wait_for(lk, std::chrono::milliseconds(milliseconds)) == std::cv_status::no_timeout || + g_program_running == true) + { + return true; + } + return false; } int main(int argc, char* argv[]) @@ -109,13 +129,13 @@ int main(int argc, char* argv[]) std::unique_ptr tool_comm_setup; const bool headless = true; g_my_driver.reset(new UrDriver(robot_ip, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, headless, - std::move(tool_comm_setup), CALIBRATION_CHECKSUM)); - - // Once RTDE communication is started, we have to make sure to read from the interface buffer, as - // otherwise we will get pipeline overflows. Therefor, do this directly before starting your main - // loop. - - g_my_driver->startRTDECommunication(); + std::move(tool_comm_setup))); + // Make sure that external control script is running + if (!waitForProgramRunning()) + { + URCL_LOG_ERROR("External Control script not running."); + return 1; + } // Increment depends on robot version double increment_constant = 0.0005; @@ -130,6 +150,11 @@ int main(int argc, char* argv[]) bool passed_positive_part = false; URCL_LOG_INFO("Start moving the robot"); urcl::vector6d_t joint_target = { 0, 0, 0, 0, 0, 0 }; + + // Once RTDE communication is started, we have to make sure to read from the interface buffer, as + // otherwise we will get pipeline overflows. Therefor, do this directly before starting your main + // loop. + g_my_driver->startRTDECommunication(); while (!(passed_positive_part && passed_negative_part)) { // Read latest RTDE package. This will block for a hard-coded timeout (see UrDriver), so the @@ -137,55 +162,53 @@ int main(int argc, char* argv[]) // In a real-world application this thread should be scheduled with real-time priority in order // to ensure that this is called in time. std::unique_ptr data_pkg = g_my_driver->getDataPackage(); - if (data_pkg) + if (!data_pkg) { - // Read current joint positions from robot data - if (!data_pkg->getData("actual_q", g_joint_positions)) - { - // This throwing should never happen unless misconfigured - std::string error_msg = "Did not find 'actual_q' in data sent from robot. This should not happen!"; - throw std::runtime_error(error_msg); - } + URCL_LOG_WARN("Could not get fresh data package from robot"); + return 1; + } + // Read current joint positions from robot data + if (!data_pkg->getData("actual_q", g_joint_positions)) + { + // This throwing should never happen unless misconfigured + std::string error_msg = "Did not find 'actual_q' in data sent from robot. This should not happen!"; + throw std::runtime_error(error_msg); + } - if (first_pass) - { - joint_target = g_joint_positions; - first_pass = false; - } + if (first_pass) + { + joint_target = g_joint_positions; + first_pass = false; + } - // Open loop control. The target is incremented with a constant each control loop - if (passed_positive_part == false) - { - increment = increment_constant; - if (g_joint_positions[5] >= 2) - { - passed_positive_part = true; - } - } - else if (passed_negative_part == false) + // Open loop control. The target is incremented with a constant each control loop + if (passed_positive_part == false) + { + increment = increment_constant; + if (g_joint_positions[5] >= 2) { - increment = -increment_constant; - if (g_joint_positions[5] <= 0) - { - passed_negative_part = true; - } + passed_positive_part = true; } - joint_target[5] += increment; - // Setting the RobotReceiveTimeout time is for example purposes only. This will make the example running more - // reliable on non-realtime systems. Use with caution in productive applications. - bool ret = g_my_driver->writeJointCommand(joint_target, comm::ControlMode::MODE_SERVOJ, - RobotReceiveTimeout::millisec(100)); - if (!ret) + } + else if (passed_negative_part == false) + { + increment = -increment_constant; + if (g_joint_positions[5] <= 0) { - URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?"); - return 1; + passed_negative_part = true; } - URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg->toString().c_str()); } - else + joint_target[5] += increment; + // Setting the RobotReceiveTimeout time is for example purposes only. This will make the example running more + // reliable on non-realtime systems. Use with caution in productive applications. + bool ret = g_my_driver->writeJointCommand(joint_target, comm::ControlMode::MODE_SERVOJ, + RobotReceiveTimeout::millisec(100)); + if (!ret) { - URCL_LOG_WARN("Could not get fresh data package from robot"); + URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?"); + return 1; } + URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg->toString().c_str()); } g_my_driver->stopControl(); URCL_LOG_INFO("Movement done"); diff --git a/examples/primary_pipeline.cpp b/examples/primary_pipeline.cpp index e4390d8b..e022b5d7 100644 --- a/examples/primary_pipeline.cpp +++ b/examples/primary_pipeline.cpp @@ -55,7 +55,7 @@ int main(int argc, char* argv[]) second_to_run = std::stoi(argv[2]); } - // First of all, we need a stream that connects to the robot + // First of all, we need a stream that connects to the robot's primary interface comm::URStream primary_stream(robot_ip, urcl::primary_interface::UR_PRIMARY_PORT); // This will parse the primary packages @@ -63,11 +63,12 @@ int main(int argc, char* argv[]) // The producer needs both, the stream and the parser to fully work comm::URProducer prod(primary_stream, parser); + + // Connect to the stream prod.setupProducer(); // The shell consumer will print the package contents to the shell - std::unique_ptr> consumer; - consumer.reset(new comm::ShellConsumer()); + auto consumer = std::make_unique>(); // The notifer will be called at some points during connection setup / loss. This isn't fully // implemented atm. diff --git a/examples/primary_pipeline_calibration.cpp b/examples/primary_pipeline_calibration.cpp index 9f54e2e8..bb1baed4 100644 --- a/examples/primary_pipeline_calibration.cpp +++ b/examples/primary_pipeline_calibration.cpp @@ -31,8 +31,11 @@ class CalibrationConsumer : public urcl::comm::IConsumer product) { + // Try to cast the product to a KinematicsInfo package. If that succeeds, we handle the + // calibration information. auto kin_info = std::dynamic_pointer_cast(product); if (kin_info != nullptr) { @@ -105,11 +108,11 @@ int main(int argc, char* argv[]) if (calib_consumer.isCalibrated()) { - printf("The robot on IP: %s is calibrated\n", robot_ip.c_str()); + printf("The robot on IP: %s is calibrated.\n", robot_ip.c_str()); } else { - printf("The robot controller on IP: %s do not have a valid calibration\n", robot_ip.c_str()); + printf("The robot controller on IP: %s does not have a valid calibration.\n", robot_ip.c_str()); printf("Remeber to turn on the robot to get calibration stored on the robot!\n"); } diff --git a/examples/rtde_client.cpp b/examples/rtde_client.cpp index 01a1bb40..0972aafc 100644 --- a/examples/rtde_client.cpp +++ b/examples/rtde_client.cpp @@ -27,6 +27,7 @@ #include +#include #include #include #include @@ -40,6 +41,20 @@ const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt"; const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; const std::chrono::milliseconds READ_TIMEOUT{ 100 }; +void printFraction(const double fraction, const std::string& label, const size_t width = 20) +{ + std::cout << "\r" << label << ": ["; + for (size_t i = 0; i < std::ceil(fraction * width); i++) + { + std::cout << "#"; + } + for (size_t i = 0; i < std::floor((1.0 - fraction) * width); i++) + { + std::cout << "-"; + } + std::cout << "]" << std::flush; +} + int main(int argc, char* argv[]) { // Parse the ip arguments if given @@ -56,13 +71,14 @@ int main(int argc, char* argv[]) second_to_run = std::stoi(argv[2]); } - // TODO: Write good docstring for notifier comm::INotifier notifier; - rtde_interface::RTDEClient my_client(robot_ip, notifier, OUTPUT_RECIPE, INPUT_RECIPE); + const double rtde_frequency = 50; // Hz + rtde_interface::RTDEClient my_client(robot_ip, notifier, OUTPUT_RECIPE, INPUT_RECIPE, rtde_frequency); my_client.init(); // We will use the speed_slider_fraction as an example how to write to RTDE double speed_slider_fraction = 1.0; + double target_speed_fraction = 1.0; double speed_slider_increment = 0.01; // Once RTDE communication is started, we have to make sure to read from the interface buffer, as @@ -70,8 +86,10 @@ int main(int argc, char* argv[]) // loop. my_client.start(); - unsigned long start_time = clock(); - while (second_to_run < 0 || ((clock() - start_time) / CLOCKS_PER_SEC) < static_cast(second_to_run)) + auto start_time = std::chrono::steady_clock::now(); + while (second_to_run <= 0 || + std::chrono::duration_cast(std::chrono::steady_clock::now() - start_time).count() < + second_to_run) { // Read latest RTDE package. This will block for READ_TIMEOUT, so the // robot will effectively be in charge of setting the frequency of this loop unless RTDE @@ -81,22 +99,19 @@ int main(int argc, char* argv[]) std::unique_ptr data_pkg = my_client.getDataPackage(READ_TIMEOUT); if (data_pkg) { - std::cout << data_pkg->toString() << std::endl; + // Data fields in the data package are accessed by their name. Only names present in the + // output recipe can be accessed. Otherwise this function will return false. + data_pkg->getData("target_speed_fraction", target_speed_fraction); + printFraction(target_speed_fraction, "target_speed_fraction"); } else { + // The client isn't connected properly anymore / doesn't receive any data anymore. Stop the + // program. std::cout << "Could not get fresh data package from robot" << std::endl; return 1; } - if (!my_client.getWriter().sendSpeedSlider(speed_slider_fraction)) - { - // This will happen for example, when the required keys are not configured inside the input - // recipe. - std::cout << "\033[1;31mSending RTDE data failed." << "\033[0m\n" << std::endl; - return 1; - } - // Change the speed slider so that it will move between 0 and 1 all the time. This is for // demonstration purposes only and gains no real value. if (speed_slider_increment > 0) @@ -111,6 +126,14 @@ int main(int argc, char* argv[]) speed_slider_increment *= -1; } speed_slider_fraction += speed_slider_increment; + + if (!my_client.getWriter().sendSpeedSlider(speed_slider_fraction)) + { + // This will happen for example, when the required keys are not configured inside the input + // recipe. + std::cout << "\033[1;31mSending RTDE data failed." << "\033[0m\n" << std::endl; + return 1; + } } // Resetting the speedslider back to 100% diff --git a/examples/tool_contact_example.cpp b/examples/tool_contact_example.cpp index 35bf739a..b28e878d 100644 --- a/examples/tool_contact_example.cpp +++ b/examples/tool_contact_example.cpp @@ -46,18 +46,26 @@ const std::string DEFAULT_ROBOT_IP = "192.168.56.101"; const std::string SCRIPT_FILE = "resources/external_control.urscript"; const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt"; const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; -const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542"; std::unique_ptr g_my_driver; std::unique_ptr g_my_dashboard; -bool g_tool_contact_result_triggered; +std::atomic g_tool_contact_result_triggered; control::ToolContactResult g_tool_contact_result; +std::atomic g_program_running; +std::condition_variable g_program_running_cv; +std::mutex g_program_running_mutex; // We need a callback function to register. See UrDriver's parameters for details. void handleRobotProgramState(bool program_running) { // Print the text in green so we see it better std::cout << "\033[1;32mProgram running: " << std::boolalpha << program_running << "\033[0m\n" << std::endl; + g_program_running = program_running; + if (program_running) + { + std::lock_guard lk(g_program_running_mutex); + g_program_running_cv.notify_one(); + } } void handleToolContactResult(control::ToolContactResult result) @@ -68,6 +76,17 @@ void handleToolContactResult(control::ToolContactResult result) g_tool_contact_result_triggered = true; } +bool waitForProgramRunning(int milliseconds = 100) +{ + std::unique_lock lk(g_program_running_mutex); + if (g_program_running_cv.wait_for(lk, std::chrono::milliseconds(milliseconds)) == std::cv_status::no_timeout || + g_program_running == true) + { + return true; + } + return false; +} + int main(int argc, char* argv[]) { urcl::setLogLevel(urcl::LogLevel::INFO); @@ -121,68 +140,43 @@ int main(int argc, char* argv[]) URCL_LOG_ERROR("Could not send BrakeRelease command"); return 1; } - // Now the robot is ready to receive a program + std::unique_ptr tool_comm_setup; const bool headless = true; g_my_driver.reset(new UrDriver(robot_ip, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, headless, - std::move(tool_comm_setup), CALIBRATION_CHECKSUM)); - + std::move(tool_comm_setup))); + if (!waitForProgramRunning(1000)) + { + std::cout << "Program did not start running. Is the robot in remote control?" << std::endl; + return 1; + } g_my_driver->registerToolContactResultCallback(&handleToolContactResult); - - // Once RTDE communication is started, we have to make sure to read from the interface buffer, as - // otherwise we will get pipeline overflows. Therefor, do this directly before starting your main - // loop. - g_my_driver->startRTDECommunication(); + g_my_driver->startToolContact(); // This will move the robot downward in the z direction of the base until a tool contact is detected or seconds_to_run // is reached - std::chrono::duration time_done(0); - std::chrono::duration timeout(second_to_run); - vector6d_t tcp_speed = { 0.0, 0.0, -0.02, 0.0, 0.0, 0.0 }; - auto stopwatch_last = std::chrono::steady_clock::now(); - auto stopwatch_now = stopwatch_last; - g_my_driver->startToolContact(); - - while (true) + const vector6d_t tcp_speed = { 0.0, 0.0, -0.02, 0.0, 0.0, 0.0 }; + auto start_time = std::chrono::system_clock::now(); + while (second_to_run.count() < 0 || (std::chrono::system_clock::now() - start_time) < second_to_run) { - // Read latest RTDE package. This will block for a hard-coded timeout (see UrDriver), so the - // robot will effectively be in charge of setting the frequency of this loop. - // In a real-world application this thread should be scheduled with real-time priority in order - // to ensure that this is called in time. - std::unique_ptr data_pkg = g_my_driver->getDataPackage(); - if (data_pkg) + // Setting the RobotReceiveTimeout time is for example purposes only. This will make the example running more + // reliable on non-realtime systems. Use with caution in productive applications. + bool ret = + g_my_driver->writeJointCommand(tcp_speed, comm::ControlMode::MODE_SPEEDL, RobotReceiveTimeout::millisec(100)); + if (!ret) { - // Setting the RobotReceiveTimeout time is for example purposes only. This will make the example running more - // reliable on non-realtime systems. Use with caution in productive applications. - bool ret = - g_my_driver->writeJointCommand(tcp_speed, comm::ControlMode::MODE_SPEEDL, RobotReceiveTimeout::millisec(100)); - if (!ret) - { - URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?"); - return 1; - } - - if (g_tool_contact_result_triggered) - { - URCL_LOG_INFO("Tool contact result triggered. Received tool contact result %i.", - toUnderlying(g_tool_contact_result)); - break; - } - - if (time_done > timeout && second_to_run.count() != 0) - { - URCL_LOG_INFO("Timed out before reaching tool contact."); - break; - } + URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?"); + return 1; } - else + + if (g_tool_contact_result_triggered) { - URCL_LOG_WARN("Could not get fresh data package from robot"); + URCL_LOG_INFO("Tool contact detected"); + break; } - - stopwatch_now = std::chrono::steady_clock::now(); - time_done += stopwatch_now - stopwatch_last; - stopwatch_last = stopwatch_now; } + URCL_LOG_INFO("Timed out before reaching tool contact."); + g_my_driver->stopControl(); + return 0; } diff --git a/examples/trajectory_point_interface.cpp b/examples/trajectory_point_interface.cpp index 8138598f..19ba0fbe 100644 --- a/examples/trajectory_point_interface.cpp +++ b/examples/trajectory_point_interface.cpp @@ -42,18 +42,38 @@ const std::string DEFAULT_ROBOT_IP = "192.168.56.101"; const std::string SCRIPT_FILE = "resources/external_control.urscript"; const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt"; const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; -const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542"; std::unique_ptr g_my_dashboard; std::unique_ptr g_my_driver; std::atomic g_trajectory_done = false; +std::atomic g_program_running; +std::condition_variable g_program_running_cv; +std::mutex g_program_running_mutex; + +bool waitForProgramRunning(int milliseconds = 100) +{ + std::unique_lock lk(g_program_running_mutex); + if (g_program_running_cv.wait_for(lk, std::chrono::milliseconds(milliseconds)) == std::cv_status::no_timeout || + g_program_running == true) + { + return true; + } + return false; +} + // We need a callback function to register. See UrDriver's parameters for details. void handleRobotProgramState(bool program_running) { // Print the text in green so we see it better std::cout << "\033[1;32mProgram running: " << std::boolalpha << program_running << "\033[0m\n" << std::endl; + g_program_running = program_running; + if (program_running) + { + std::lock_guard lk(g_program_running_mutex); + g_program_running_cv.notify_one(); + } } void trajDoneCallback(const urcl::control::TrajectoryResult& result) @@ -113,7 +133,12 @@ int main(int argc, char* argv[]) std::unique_ptr tool_comm_setup; const bool headless = true; g_my_driver.reset(new urcl::UrDriver(robot_ip, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, - headless, std::move(tool_comm_setup), CALIBRATION_CHECKSUM)); + headless, std::move(tool_comm_setup))); + if (!waitForProgramRunning(1000)) + { + URCL_LOG_ERROR("Program did not start running. Is the robot in remote control?"); + return 1; + } // --------------- INITIALIZATION END ------------------- g_my_driver->registerTrajectoryDoneCallback(&trajDoneCallback); @@ -218,7 +243,7 @@ int main(int argc, char* argv[]) g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP); } } - // --------------- END MOVEJ TRAJECTORY ------------------- + // --------------- END SPLINE TRAJECTORY ------------------- g_my_driver->stopControl(); return 0; diff --git a/include/ur_client_library/ur/dashboard_client.h b/include/ur_client_library/ur/dashboard_client.h index 579c7bb1..0d12d954 100644 --- a/include/ur_client_library/ur/dashboard_client.h +++ b/include/ur_client_library/ur/dashboard_client.h @@ -89,7 +89,7 @@ class DashboardClient : public comm::TCPSocket * \brief Sends command and compare it with the expected answer * * \param command Command that will be sent to the server. - * \param expected Expected response + * \param expected Expected response as a regex string * * \return True if the reply to the command is as expected */ @@ -99,7 +99,7 @@ class DashboardClient : public comm::TCPSocket * \brief Sends command and compare it with the expected answer * * \param command Command that will be sent to the server. - * \param expected Expected response + * \param expected Expected response as a regex string * * \throws UrException if the received answer does not match the expected one. * diff --git a/run_examples.sh b/run_examples.sh index 72edb232..2b91e3de 100755 --- a/run_examples.sh +++ b/run_examples.sh @@ -13,13 +13,14 @@ for file in $folder_path/*; do echo $file # Check if the file is executable if [[ -f "$file" && -x "$file" ]]; then + printf "\n#### Executing '$file'\n" # Execute the file ./"$file" $@ # Check the exit status exit_status=$? if [[ $exit_status -ne 0 ]]; then - echo "Execution of '$file' failed with exit status $exit_status." - exit 1 + echo "Execution of '$file' failed with exit status $exit_status." + exit 1 fi # Delay for 10 seconds to avoid too fast reconnects echo "Sleep 10" diff --git a/src/helpers.cpp b/src/helpers.cpp index 2cd1eab1..6697be5b 100644 --- a/src/helpers.cpp +++ b/src/helpers.cpp @@ -33,6 +33,12 @@ #include #include +// clang-format off +// We want to keep the URL in one line to avoid formatting issues. This will make it easier to +// extract the URL for an automatic check. +const std::string RT_DOC_URL = "https://docs.universal-robots.com/Universal_Robots_ROS_Documentation/doc/ur_client_library/doc/real_time.html"; +// clang-format on + namespace urcl { bool setFiFoScheduling(pthread_t& thread, const int priority) @@ -48,8 +54,8 @@ bool setFiFoScheduling(pthread_t& thread, const int priority) { URCL_LOG_WARN("Your system/user seems not to be setup for FIFO scheduling. We recommend using a lowlatency " "kernel with FIFO scheduling. See " - "https://github.com/UniversalRobots/Universal_Robots_ROS_Driver/blob/master/ur_robot_driver/" - "doc/real_time.md for details."); + "%s for details.", + RT_DOC_URL.c_str()); break; } default: