diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 5538e9f38..7f7284a2a 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -15,7 +15,7 @@ repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.5.0 + rev: v4.6.0 hooks: - id: check-added-large-files - id: check-ast @@ -33,13 +33,13 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v3.15.1 + rev: v3.16.0 hooks: - id: pyupgrade args: [--py36-plus] - repo: https://github.com/psf/black - rev: 24.2.0 + rev: 24.4.2 hooks: - id: black args: ["--line-length=100"] @@ -51,7 +51,7 @@ repos: args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D401,D404"] - repo: https://github.com/pycqa/flake8 - rev: 7.0.0 + rev: 7.1.0 hooks: - id: flake8 args: ["--ignore=E501,W503"] @@ -116,7 +116,7 @@ repos: stages: [commit] entry: ament_copyright language: system - args: ['--exclude', 'ur_robot_driver/doc/conf.py'] + args: ['--exclude', 'ur_robot_driver/doc/conf.py', 'ur_calibration/doc/conf.py'] # Docs - RestructuredText hooks - repo: https://github.com/PyCQA/doc8 @@ -135,8 +135,8 @@ repos: # Spellcheck in comments and docs # skipping of *.svg files is not working... - repo: https://github.com/codespell-project/codespell - rev: v2.2.6 + rev: v2.3.0 hooks: - id: codespell - args: ['--write-changes'] + args: ['--write-changes', '-L bootup,assertIn'] exclude: \.(svg|pyc|drawio)$ diff --git a/ur/CHANGELOG.rst b/ur/CHANGELOG.rst index 97ec20612..d7568af14 100644 --- a/ur/CHANGELOG.rst +++ b/ur/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package ur ^^^^^^^^^^^^^^^^^^^^^^^^ +2.4.8 (2024-07-01) +------------------ + +2.4.7 (2024-06-19) +------------------ + +2.4.6 (2024-06-17) +------------------ + 2.4.5 (2024-05-16) ------------------ diff --git a/ur/package.xml b/ur/package.xml index 11f8431ff..c442c76eb 100644 --- a/ur/package.xml +++ b/ur/package.xml @@ -2,7 +2,7 @@ ur - 2.4.5 + 2.4.8 Metapackage for universal robots Felix Exner Robert Wilbrandt diff --git a/ur_calibration/CHANGELOG.rst b/ur_calibration/CHANGELOG.rst index 11eae704b..3cafc1f5b 100644 --- a/ur_calibration/CHANGELOG.rst +++ b/ur_calibration/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package ur_calibration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.4.8 (2024-07-01) +------------------ + +2.4.7 (2024-06-19) +------------------ + +2.4.6 (2024-06-17) +------------------ +* Fix calibration (`#1017 `_) +* Restructure documentation for full stack documentation (`#984 `_) +* Contributors: Felix Exner (fexner), Vincenzo Di Pentima + 2.4.5 (2024-05-16) ------------------ diff --git a/ur_calibration/doc/algorithm.rst b/ur_calibration/doc/algorithm.rst new file mode 100644 index 000000000..036439030 --- /dev/null +++ b/ur_calibration/doc/algorithm.rst @@ -0,0 +1,97 @@ +Calibration correction algorithm +================================ + +When extracting the robot's calibration there will be a set of Denavit–Hartenberg (DH) parameters +describing the robot. Due to the calibration process they can seem a bit unintuitive since the +``d``-parameter of the second and third joint can be quite large on those. + +For example, let's consider the following dh parameters taken from a real robot: + +.. code-block:: yaml + + # joint1 joint2 joint3 joint4 joint5 joint6 + dh_theta: [-2.4147806894359e-07 1.60233952386695 -1.68607190752171 0.0837331147700119 -1.01260355871158e-07 3.91986209186124e-08 ] + dh_a: [ 2.12234865571206e-05 0.0193171326277006 -0.569251663611088 -4.61409023720934e-05 -6.39280053471802e-05 0 ] + dh_d: [ 0.180539811714259 439.140974079901 -446.027059806332 7.0603368964236 0.119811341150314 0.115670917257426 ] + dh_alpha: [ 1.57014608044242 0.0013941666682559 0.00693818880325995 1.56998468543761 -1.57038520649543 0 ] + +One can see that the upper arm is placed 439 m out of the base link with the lower arm being 7 m to +the other side. + +We can build a robot description that splits each DH segment into two links: One for ``d`` and +``theta`` representing the rotational part of the joint and one for ``a`` and ``alpha`` +representing the "passive" part of the joint displacing the next link. +:numref:`calibration_example` shows (a part of) the description matching the parameters above. The +arm links are left out of the image as they are really far away. + +.. _calibration_example: +.. figure:: calibration_example.png + :alt: Example of a calibrated model + + This shows an example calibrated model when using the DH parameters directly as explained above. + The two arm links are virtually displaced very far from the physical robot while the TCP ends up + at the correct location again + + +For explaining the correction algorithm, we will use an artificial set of DH parameters for a +UR10e: + +.. code-block:: yaml + + # join1 joint2 joint3 joint4 joint5 joint6 + dh_theta: [0 0 0 0 0 0 ] + dh_a: [0 -0.6127 -0.57155 0 0 0 ] + dh_d: [0.1807 1 0.5 -1.32585 0.11985 0.11655] + dh_alpha: [1.570796327 0.2 0 1.570796327 -1.570796327 0 ] + +The resulting uncorrected model can be seen in :numref:`calibration_uncorrected`. The upper arm is +placed 1 m to the left of the shoulder, the upper arm is placed 0.5 m further out and there's an +added ``alpha`` rotation in joint2. + +Note: This is not a valid calibration, so when placing this "calibration" on a robot and using the +correction, we won't get correct tcp pose results. This only serves as a exaggerated example. + +.. _calibration_uncorrected: +.. figure:: calibration_uncorrected.png + :alt: Exaggerated calibrated model + + This shows an artificial calibration only to show the algorithm. This is no valid calibration! + +In :numref:`calibration_uncorrected` the separation between the two DH components can be seen quite +clearly. joint2's ``d`` and ``theta`` parameters are represented by ``upper_arm_d`` and its ``a`` +and ``alpha`` parameters result in ``upper_arm_a``. + +The "correction" step tries to bring the two arm segments back to their physical representation. +In principle, the d parameter to zero, first. With this change, +the kinematic structure gets destroyed, which has to be corrected: + +- With setting ``d`` to 0, both the start (``upper_arm_d``) and end (``upper_arm_a``) points of the + passive segment move along the joint's rotational axis. Instead, the end point of the passive + segment has to move along the rotational axis of the next segment. This requires adapting + ``a`` and ``theta``, if the two rotational axes are not parallel. + +- The length of moving along the next segment's rotational axis is calculated by intersecting + the next rotational axis with the XY-plane of the moved ``_d`` frame. This gets subtracted from + the next joint's ``d`` parameter. + +Note that the parameters from this model are not strict DH parameters anymore, as the two frames at +the tip of the upper and lower arm have to get an additional rotation to compensate the change of +the arm segment orientation, when the tip is moving along its rotation axis. + +The resulting "DH parameters" are then reassembled into six individual transforms that can become +the six frames of the robot's kinematic chain. This is exported in a yaml representation and gets +read by the description package. + +Also, no correction of the visual meshes is performed. Strictly speaking, the visual +model is not 100 % correct, but with a calibrated model and ideal meshes this cannot be achieved and +the inaccuracies introduced by this are considered negligible. + +The example as visualized in :numref:`calibration_example` looks as follows if a description with +the correct parameters is loaded: + +.. figure:: calibration_example_corrected.png + :alt: Example with corrected kinematics structure + + This shows the model from :numref:`calibration_example` with the calibration correction applied. + The robot is correctly assembled and the ``base->tool0`` transformation is exactly the same as + on the robot controller. diff --git a/ur_calibration/doc/calibration_example.png b/ur_calibration/doc/calibration_example.png new file mode 100644 index 000000000..6185a198c Binary files /dev/null and b/ur_calibration/doc/calibration_example.png differ diff --git a/ur_calibration/doc/calibration_example_corrected.png b/ur_calibration/doc/calibration_example_corrected.png new file mode 100644 index 000000000..a90c695d3 Binary files /dev/null and b/ur_calibration/doc/calibration_example_corrected.png differ diff --git a/ur_calibration/doc/calibration_uncorrected.png b/ur_calibration/doc/calibration_uncorrected.png new file mode 100644 index 000000000..73b2a46dc Binary files /dev/null and b/ur_calibration/doc/calibration_uncorrected.png differ diff --git a/ur_calibration/doc/conf.py b/ur_calibration/doc/conf.py new file mode 100644 index 000000000..c0b683e07 --- /dev/null +++ b/ur_calibration/doc/conf.py @@ -0,0 +1,85 @@ +project = "ur_calibration" +copyright = "2022, Universal Robots A/S" +author = "Felix Exner" + +# The short X.Y version +version = "" +# The full version, including alpha/beta/rc tags +release = "" + +# -- General configuration --------------------------------------------------- + +# If your documentation needs a minimal Sphinx version, state it here. +# +# needs_sphinx = '1.0' + +# Add any Sphinx extension module names here, as strings. They can be +# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom +# ones. +extensions = [] + +# Add any paths that contain templates here, relative to this directory. +templates_path = ["_templates"] + +# The suffix(es) of source filenames. +# You can specify multiple suffix as a list of string: +# +source_suffix = [".rst"] + +# The master toctree document. +master_doc = "index" + +numfig = True + +# The language for content autogenerated by Sphinx. Refer to documentation +# for a list of supported languages. +# +# This is also used if you do content translation via gettext catalogs. +# Usually you set "language" from the command line for these cases. +language = None + +# List of patterns, relative to source directory, that match files and +# directories to ignore when looking for source files. +# This pattern also affects html_static_path and html_extra_path. +exclude_patterns = ["_build", "Thumbs.db", ".DS_Store"] + +# The name of the Pygments (syntax highlighting) style to use. +pygments_style = None + + +# -- Options for HTML output ------------------------------------------------- + +# The theme to use for HTML and HTML Help pages. See the documentation for +# a list of builtin themes. +# +html_theme = "alabaster" + +# Theme options are theme-specific and customize the look and feel of a theme +# further. For a list of options available for each theme, see the +# documentation. +# +# html_theme_options = {} + +# Add any paths that contain custom static files (such as style sheets) here, +# relative to this directory. They are copied after the builtin static files, +# so a file named "default.css" will overwrite the builtin "default.css". +html_static_path = ["_static"] + +# Custom sidebar templates, must be a dictionary that maps document names +# to template names. +# +# The default sidebars (for documents that don't match any pattern) are +# defined by theme itself. Builtin themes are using these templates by +# default: ``['localtoc.html', 'relations.html', 'sourcelink.html', +# 'searchbox.html']``. +# +# html_sidebars = {} + + +# -- Options for HTMLHelp output --------------------------------------------- + +# Output file base name for HTML help builder. +htmlhelp_basename = "ur_calibration_doc" + + +# -- Options for LaTeX output ------------------------------------------------ diff --git a/ur_calibration/doc/index.rst b/ur_calibration/doc/index.rst index d8056e35a..c91768eca 100644 --- a/ur_calibration/doc/index.rst +++ b/ur_calibration/doc/index.rst @@ -10,36 +10,8 @@ make use of this in ROS, you first have to extract the calibration information f Though this step is not necessary, to control the robot using this driver, it is highly recommended to do so, as end effector positions might be off in the magnitude of centimeters. -Nodes ------ +.. toctree:: + :maxdepth: 2 -calibration_correction -^^^^^^^^^^^^^^^^^^^^^^ - -This node extracts calibration information directly from a robot, calculates the URDF correction and -saves it into a .yaml file. - -In the launch folder of the ur_calibration package is a helper script: - -.. code-block:: bash - - $ ros2 launch ur_calibration calibration_correction.launch.py \ - robot_ip:= target_filename:="${HOME}/my_robot_calibration.yaml" - -For the parameter ``robot_ip`` insert the IP address on which the ROS pc can reach the robot. As -``target_filename`` provide an absolute path where the result will be saved to. - -With that, you can launch your specific robot with the correct calibration using - -.. code-block:: bash - - $ ros2 launch ur_robot_driver ur_control.launch.py \ - ur_type:=ur5e \ - robot_ip:=192.168.56.101 \ - kinematics_params_file:="${HOME}/my_robot_calibration.yaml" - -Adapt the robot model matching to your robot. - -Ideally, you would create a package for your custom workcell, as explained in `the custom workcell -tutorial -`_. + usage + algorithm diff --git a/ur_calibration/doc/usage.rst b/ur_calibration/doc/usage.rst new file mode 100644 index 000000000..ecf4b2384 --- /dev/null +++ b/ur_calibration/doc/usage.rst @@ -0,0 +1,28 @@ +Usage +===== + +To use the calibration correction this package provides a launchfile that extracts calibration +information directly from a robot, calculates the URDF correction and saves it into a .yaml file: + +.. code-block:: bash + + $ ros2 launch ur_calibration calibration_correction.launch.py \ + robot_ip:= target_filename:="${HOME}/my_robot_calibration.yaml" + +For the parameter ``robot_ip`` insert the IP address on which the ROS pc can reach the robot. As +``target_filename`` provide an absolute path where the result will be saved to. + +With that, you can launch your specific robot with the correct calibration using + +.. code-block:: bash + + $ ros2 launch ur_robot_driver ur_control.launch.py \ + ur_type:=ur5e \ + robot_ip:=192.168.56.101 \ + kinematics_params_file:="${HOME}/my_robot_calibration.yaml" + +Adapt the robot model matching to your robot. + +Ideally, you would create a package for your custom workcell, as explained in `the custom workcell +tutorial +`_. diff --git a/ur_calibration/package.xml b/ur_calibration/package.xml index 55fd38fc8..05bde8fe5 100644 --- a/ur_calibration/package.xml +++ b/ur_calibration/package.xml @@ -1,7 +1,7 @@ ur_calibration - 2.4.5 + 2.4.8 Package for extracting the factory calibration from a UR robot and change it such that it can be used by ur_description to gain a correct URDF Felix Exner diff --git a/ur_calibration/src/calibration.cpp b/ur_calibration/src/calibration.cpp index 0c9108b35..9a680d856 100644 --- a/ur_calibration/src/calibration.cpp +++ b/ur_calibration/src/calibration.cpp @@ -73,72 +73,87 @@ void Calibration::correctAxis(const size_t link_index) // the kinematic structure gets destroyed, which has to be corrected: // - With setting d to 0, both the start and end points of the passive segment move along the // rotational axis of the start segment. Instead, the end point of the passive segment has to - // move along the rotational axis of the next segment. This creates a change in a and and theta, if + // move along the rotational axis of the next segment. This creates a change in a and theta, if // the two rotational axes are not parallel. // // - The length of moving along the next segment's rotational axis is calculated by intersecting // the rotational axis with the XY-plane of the first segment. + // + auto& d_theta_segment = chain_[2 * link_index]; + auto& a_alpha_segment = chain_[2 * link_index + 1]; + + auto& d = d_theta_segment(2, 3); + auto& a = a_alpha_segment(0, 3); - if (chain_[2 * link_index](2, 3) == 0.0) { + if (d == 0.0) { // Nothing to do here. return; } - Eigen::Matrix4d fk_current = Eigen::Matrix4d::Identity(); - Eigen::Vector3d current_passive = Eigen::Vector3d::Zero(); + // Start of the next joint's d_theta segment relative to the joint before the current one + Eigen::Matrix4d next_joint_root = Eigen::Matrix4d::Identity(); + next_joint_root *= d_theta_segment * a_alpha_segment; - Eigen::Matrix4d fk_next_passive = Eigen::Matrix4d::Identity(); - fk_next_passive *= chain_[link_index * 2] * chain_[link_index * 2 + 1]; - Eigen::Vector3d eigen_passive = fk_next_passive.topRightCorner(3, 1); + Eigen::Vector3d next_root_position = next_joint_root.topRightCorner(3, 1); - Eigen::Vector3d eigen_next = (fk_next_passive * chain_[(link_index + 1) * 2]).topRightCorner(3, 1); + const auto& next_d_theta_segment = chain_[(link_index + 1) * 2]; + Eigen::Vector3d next_d_theta_end = (next_joint_root * next_d_theta_segment).topRightCorner(3, 1); // Construct a representation of the next segment's rotational axis - Eigen::ParametrizedLine next_line; - next_line = Eigen::ParametrizedLine::Through(eigen_passive, eigen_next); + Eigen::ParametrizedLine next_rotation_axis; + next_rotation_axis = Eigen::ParametrizedLine::Through(next_root_position, next_d_theta_end); - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("ur_calibration"), "next_line:" << std::endl - << "Base:" << std::endl - << next_line.origin() << std::endl - << "Direction:" << std::endl - << next_line.direction()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("ur_calibration"), "next rotation axis:" << std::endl + << "Base:" << std::endl + << next_rotation_axis.origin() + << std::endl + << "Direction:" << std::endl + << next_rotation_axis.direction()); // XY-Plane of first segment's start - Eigen::Hyperplane plane(fk_current.topLeftCorner(3, 3) * Eigen::Vector3d(0, 0, 1), current_passive); - - // Intersect the rotation axis with the XY-Plane. We use both notations, the length and - // intersection point, as we will need both. The intersection_param is the length moving along the - // plane (change in the next segment's d param), while the intersection point will be used for - // calculating the new angle theta. - double intersection_param = next_line.intersectionParameter(plane); - Eigen::Vector3d intersection = next_line.intersectionPoint(plane) - current_passive; - double new_theta = std::atan(intersection.y() / intersection.x()); + Eigen::Hyperplane plane(Eigen::Vector3d(0, 0, 1), Eigen::Vector3d::Zero()); + + // Intersect the rotation axis of the next joint with the XY-Plane. + // * The intersection_param is the length moving along the rotation axis until intersecting the plane. + // * The intersection point will be used for calculating the new angle theta. + double intersection_param = next_rotation_axis.intersectionParameter(plane); + Eigen::Vector3d intersection_point = next_rotation_axis.intersectionPoint(plane); + + // A non-zero a parameter will result in an intersection point at (a, 0) even without any + // additional rotations. This effect has to be subtracted from the resulting theta value. + double subtraction_angle = 0.0; + if (std::abs(a) > 0) { + // This is pi + subtraction_angle = atan(1) * 4; + } + double new_theta = std::atan2(intersection_point.y(), intersection_point.x()) - subtraction_angle; // Upper and lower arm segments on URs all have negative length due to dh params - double new_length = -1 * intersection.norm(); - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("ur_calibration"), "Wrist line intersecting at " << std::endl << intersection); + double new_link_length = -1 * intersection_point.norm(); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("ur_calibration"), "Next joint's rotation axis intersecting at " + << std::endl + << intersection_point); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("ur_calibration"), "Angle is " << new_theta); - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("ur_calibration"), "Length is " << new_length); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("ur_calibration"), "Length is " << new_link_length); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("ur_calibration"), "Intersection param is " << intersection_param); // as we move the passive segment towards the first segment, we have to move away the next segment // again, to keep the same kinematic structure. - double sign_dir = next_line.direction().z() > 0 ? 1.0 : -1.0; + double sign_dir = next_rotation_axis.direction().z() > 0 ? 1.0 : -1.0; double distance_correction = intersection_param * sign_dir; // Set d parameter of the first segment to 0 and theta to the calculated new angle // Correct arm segment length and angle - chain_[2 * link_index](2, 3) = 0.0; - chain_[2 * link_index].topLeftCorner(3, 3) = - Eigen::AngleAxisd(new_theta, Eigen::Vector3d::UnitZ()).toRotationMatrix(); + d = 0.0; + d_theta_segment.topLeftCorner(3, 3) = Eigen::AngleAxisd(new_theta, Eigen::Vector3d::UnitZ()).toRotationMatrix(); // Correct arm segment length and angle - chain_[2 * link_index + 1](0, 3) = new_length; - chain_[2 * link_index + 1].topLeftCorner(3, 3) = + a = new_link_length; + a_alpha_segment.topLeftCorner(3, 3) = Eigen::AngleAxisd(robot_parameters_.segments_[link_index].theta_ - new_theta, Eigen::Vector3d::UnitZ()) .toRotationMatrix() * Eigen::AngleAxisd(robot_parameters_.segments_[link_index].alpha_, Eigen::Vector3d::UnitX()).toRotationMatrix(); - // Correct next joint + // Correct next joint's d parameter chain_[2 * link_index + 2](2, 3) -= distance_correction; } diff --git a/ur_calibration/test/calibration_test.cpp b/ur_calibration/test/calibration_test.cpp index f6910fe0f..e5f0685b0 100644 --- a/ur_calibration/test/calibration_test.cpp +++ b/ur_calibration/test/calibration_test.cpp @@ -42,12 +42,16 @@ //---------------------------------------------------------------------- #include +#include +#include #include using ur_calibration::Calibration; using ur_calibration::DHRobot; using ur_calibration::DHSegment; +using CalibrationMat = Eigen::Matrix; + namespace { /* @@ -67,12 +71,36 @@ void doubleEqVec(const Eigen::Matrix vec1, const Eigen::Matrix } // namespace -class CalibrationTest : public ::testing::Test +DHRobot model_from_dh(std::array d, std::array a, std::array theta, + std::array alpha) +{ + DHRobot robot; + for (size_t i = 0; i < 6; ++i) { + robot.segments_.emplace_back(DHSegment(d[i], a[i], theta[i], alpha[i])); + } + return robot; +} + +class CalibrationTest : public ::testing::TestWithParam> { public: void SetUp() { - Test::SetUp(); + robot_models_.insert({ "ur10_ideal", model_from_dh({ 0.1273, 0, 0, 0.163941, 0.1157, 0.0922 }, // d + { 0, -0.612, -0.5723, 0, 0, 0 }, // a + { 0, 0, 0, 0, 0, 0 }, // theta + { pi / 2, 0, 0, pi / 2, -pi / 2, 0 } // alpha + ) }); + robot_models_.insert({ "ur10", model_from_dh({ 0.1273, 0, 0, 0.163941, 0.1157, 0.0922 }, // d + { 0, -0.612, -0.5723, 0, 0, 0 }, // a + { 0, 0, 0, 0, 0, 0 }, // theta + { pi_2, 0, 0, pi_2, -pi_2, 0 } // alpha + ) }); + robot_models_.insert({ "ur10e", model_from_dh({ 0.1807, 0, 0, 0.17415, 0.11985, 0.11655 }, // d + { 0, -0.6127, -0.57155, 0, 0, 0 }, // a + { 0, 0, 0, 0, 0, 0 }, // theta + { pi_2, 0, 0, pi_2, -pi_2, 0 } // alpha + ) }); } void TearDown() { @@ -82,24 +110,13 @@ class CalibrationTest : public ::testing::Test protected: const double pi = std::atan(1) * 4; const double pi_2 = 1.570796327; // This is what the simulated robot reports as pi/2 - DHRobot my_robot_; - DHRobot my_robot_calibration_; + std::map robot_models_; }; TEST_F(CalibrationTest, ur10_fw_kinematics_ideal) { - my_robot_.segments_.clear(); - // This is an ideal UR10 - // clang-format off - my_robot_.segments_.emplace_back(DHSegment(0.1273 , 0 , 0 , pi / 2)); - my_robot_.segments_.emplace_back(DHSegment(0 , -0.612 , 0 , 0)); - my_robot_.segments_.emplace_back(DHSegment(0 , -0.5723, 0 , 0.0)); - my_robot_.segments_.emplace_back(DHSegment(0.163941, 0 , 0 , pi / 2)); - my_robot_.segments_.emplace_back(DHSegment(0.1157 , 0 , 0 , -pi / 2)); - my_robot_.segments_.emplace_back(DHSegment(0.0922 , 0 , 0 , 0)); - // clang-format on - - Calibration calibration(my_robot_); + const auto& my_robot = robot_models_["ur10_ideal"]; + Calibration calibration(my_robot); Eigen::Matrix jointvalues; Eigen::Vector3d expected_position; @@ -107,38 +124,28 @@ TEST_F(CalibrationTest, ur10_fw_kinematics_ideal) { jointvalues << 0, 0, 0, 0, 0, 0; Eigen::Matrix4d fk = calibration.calcForwardKinematics(jointvalues); - EXPECT_DOUBLE_EQ(fk(0, 3), my_robot_.segments_[1].a_ + my_robot_.segments_[2].a_); - EXPECT_DOUBLE_EQ(fk(1, 3), -1 * (my_robot_.segments_[3].d_ + my_robot_.segments_[5].d_)); - EXPECT_DOUBLE_EQ(fk(2, 3), my_robot_.segments_[0].d_ - my_robot_.segments_[4].d_); + EXPECT_DOUBLE_EQ(fk(0, 3), my_robot.segments_[1].a_ + my_robot.segments_[2].a_); + EXPECT_DOUBLE_EQ(fk(1, 3), -1 * (my_robot.segments_[3].d_ + my_robot.segments_[5].d_)); + EXPECT_DOUBLE_EQ(fk(2, 3), my_robot.segments_[0].d_ - my_robot.segments_[4].d_); } { jointvalues << M_PI_2, -M_PI_4, M_PI_2, -M_PI_4, 0, 0; Eigen::Matrix4d fk = calibration.calcForwardKinematics(jointvalues); - expected_position << my_robot_.segments_[3].d_ + my_robot_.segments_[5].d_, - my_robot_.segments_[1].a_ / std::sqrt(2) + my_robot_.segments_[2].a_ / std::sqrt(2), - my_robot_.segments_[0].d_ - my_robot_.segments_[1].a_ / std::sqrt(2) + - my_robot_.segments_[2].a_ / std::sqrt(2) - my_robot_.segments_[4].d_; - doubleEqVec(expected_position, fk.topRightCorner(3, 1), 1e-16); + expected_position << my_robot.segments_[3].d_ + my_robot.segments_[5].d_, + my_robot.segments_[1].a_ / std::sqrt(2) + my_robot.segments_[2].a_ / std::sqrt(2), + my_robot.segments_[0].d_ - my_robot.segments_[1].a_ / std::sqrt(2) + my_robot.segments_[2].a_ / std::sqrt(2) - + my_robot.segments_[4].d_; + doubleEqVec(expected_position, fk.topRightCorner(3, 1), 1e-8); } } TEST_F(CalibrationTest, ur10_fw_kinematics_real) { // This test compares a corrected ideal model against positions taken from a simulated robot. - my_robot_.segments_.clear(); - // This is an ideal UR10 - // clang-format off - my_robot_.segments_.emplace_back(DHSegment(0.1273 , 0 , 0 , pi_2)); - my_robot_.segments_.emplace_back(DHSegment(0 , -0.612 , 0 , 0)); - my_robot_.segments_.emplace_back(DHSegment(0 , -0.5723, 0 , 0.0)); - my_robot_.segments_.emplace_back(DHSegment(0.163941, 0 , 0 , pi_2)); - my_robot_.segments_.emplace_back(DHSegment(0.1157 , 0 , 0 , -pi_2)); - my_robot_.segments_.emplace_back(DHSegment(0.0922 , 0 , 0 , 0)); - // clang-format on - - Calibration calibration(my_robot_); + const auto& my_robot = robot_models_["ur10"]; + Calibration calibration(my_robot); Eigen::Matrix jointvalues; Eigen::Vector3d expected_position; @@ -167,46 +174,21 @@ TEST_F(CalibrationTest, ur10_fw_kinematics_real) TearDown(); } -TEST_F(CalibrationTest, calibration) +TEST_P(CalibrationTest, calibration) { // This test compares the forward kinematics of the model constructed from uncorrected // parameters with the one from the corrected parameters. They are tested against random // joint values and should be equal (in a numeric sense). - my_robot_.segments_.clear(); - - // This is an ideal UR10 - // clang-format off - // d, a, theta, alpha - my_robot_.segments_.emplace_back(DHSegment(0.1273 , 0 , 0 , pi / 2)); - my_robot_.segments_.emplace_back(DHSegment(0 , -0.612 , 0 , 0)); - my_robot_.segments_.emplace_back(DHSegment(0 , -0.5723, 0 , 0.0)); - my_robot_.segments_.emplace_back(DHSegment(0.163941, 0 , 0 , pi / 2)); - my_robot_.segments_.emplace_back(DHSegment(0.1157 , 0 , 0 , -pi / 2)); - my_robot_.segments_.emplace_back(DHSegment(0.0922 , 0 , 0 , 0)); - // clang-format on - my_robot_calibration_.segments_.clear(); - // clang-format off - // d, a, theta, alpha - my_robot_calibration_.segments_.emplace_back(DHSegment( 0.00065609212979853 , 4.6311376834935676e-05 , - -7.290070070824746e-05 , 0.000211987863869334 )); - my_robot_calibration_.segments_.emplace_back(DHSegment( 1.4442162376284788 , -0.00012568315331862312 , - -0.01713897289704999 , -0.0072553625957652995)); - my_robot_calibration_.segments_.emplace_back(DHSegment( 0.854147723854608 , 0.00186216581161458 , - -0.03707159413492756 , -0.013483226769541364 )); - my_robot_calibration_.segments_.emplace_back(DHSegment(-2.2989425877563705 , 9.918593870679266e-05 , - 0.054279462160583214 , 0.0013495820227329425 )); - my_robot_calibration_.segments_.emplace_back(DHSegment(-1.573498686836816e-05 , 4.215462720453189e-06 , - 1.488984257025741e-07 , -0.001263136163679901 )); - my_robot_calibration_.segments_.emplace_back(DHSegment( 1.9072435590711256e-05 , 0 , - 1.551499479707493e-05 , 0 )); - // clang-format on + auto [robot_model, robot_calibration] = GetParam(); + auto my_robot = robot_models_.at(robot_model); + Calibration calibration(my_robot); Eigen::Matrix jointvalues; jointvalues << 0, 0, 0, 0, 0, 0; for (size_t i = 0; i < 1000; ++i) { - Calibration calibration(my_robot_ + my_robot_calibration_); + Calibration calibration(my_robot + robot_calibration); jointvalues = 2 * pi * Eigen::Matrix::Random(); Eigen::Matrix4d fk_orig = calibration.calcForwardKinematics(jointvalues); Eigen::Matrix3d rot_mat_orig = fk_orig.topLeftCorner(3, 3); @@ -222,3 +204,45 @@ TEST_F(CalibrationTest, calibration) EXPECT_NEAR(angle_error, 0.0, 1e-12); } } + +// Tests are parametrized by both, the robot model (e.g. "ur10e") and the DH diff as they are stored on the robot +// controller's calibration file. +// The test will then assemble the DH parameters using the ones from the base model and the calibration. +INSTANTIATE_TEST_SUITE_P( + CalibrationTests, CalibrationTest, + ::testing::Values( + std::make_tuple("ur10", + model_from_dh({ 0.00065609212979853, 1.4442162376284788, 0.854147723854608, -2.2989425877563705, + -1.573498686836816e-05, 1.9072435590711256e-05 }, // d + { 4.6311376834935676e-05, -0.00012568315331862312, 0.00186216581161458, + 9.918593870679266e-05, 4.215462720453189e-06, 0 }, // a + { -7.290070070824746e-05, -0.01713897289704999, -0.03707159413492756, + 0.054279462160583214, 1.488984257025741e-07, 1.551499479707493e-05 }, // theta + { 0.000211987863869334, -0.0072553625957652995, -0.013483226769541364, + 0.0013495820227329425, -0.001263136163679901, 0 } // alpha + )), + + std::make_tuple("ur10e", + model_from_dh({ -0.000144894975118076141, 303.469135666158195, -309.88394307789747, + 6.41459904397394975, -4.48232900190081995e-05, -0.00087071402790364627 }, // d + { 0.000108651068627930392, 0.240324175250532346, 0.00180628180213493472, + 2.63076149165684402e-05, 3.96638632500012715e-06, 0 }, // a + { 1.59613525316931737e-07, -0.917209621528830232, 7.12936131346499469, + 0.0710299029424392298, -1.64258976526054923e-06, + 9.17286101034808787e-08 }, // theta + { -0.000444781952841921679, -0.00160215112531214153, 0.00631917793331091861, + -0.00165055247340828437, 0.000763682515545038854, 0 } // alpha + )), + std::make_tuple("ur10e", + model_from_dh({ -0.000160188285741325043, 439.140974079900957, -446.027059806332147, + 6.88618689642360149, -3.86588496858186748e-05, -0.00087908274257374186 }, // d + { 2.12234865571205891e-05, 0.632017132627700651, 0.00229833638891230319, + -4.61409023720933908e-05, -6.39280053471801522e-05, 0 }, // a + { -2.41478068943590252e-07, 1.60233952386694556, -1.68607190752171299, + 0.0837331147700118711, -1.01260355871157781e-07, + 3.91986209186123702e-08 }, // theta + { -0.000650246557584166496, 0.00139416666825590402, 0.00693818880325995126, + -0.000811641562390219562, 0.000411120504570705592, 0 } // alpha + )) + + )); diff --git a/ur_controllers/CHANGELOG.rst b/ur_controllers/CHANGELOG.rst index 7ddef74fc..9592fd877 100644 --- a/ur_controllers/CHANGELOG.rst +++ b/ur_controllers/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package ur_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.4.8 (2024-07-01) +------------------ + +2.4.7 (2024-06-19) +------------------ + +2.4.6 (2024-06-17) +------------------ +* this simple fix should fix the goal time violated issue (`#882 `_) +* Restructure documentation for full stack documentation (`#984 `_) +* Contributors: Felix Exner (fexner), Lennart Nachtigall, Vincenzo Di Pentima + 2.4.5 (2024-05-16) ------------------ * Use latched publishing for robot_mode and safety_mode diff --git a/ur_controllers/package.xml b/ur_controllers/package.xml index 428d0e1b4..6c5daf1f6 100644 --- a/ur_controllers/package.xml +++ b/ur_controllers/package.xml @@ -2,7 +2,7 @@ ur_controllers - 2.4.5 + 2.4.8 Provides controllers that use the speed scaling interface of Universal Robots. Denis Stogl diff --git a/ur_dashboard_msgs/CHANGELOG.rst b/ur_dashboard_msgs/CHANGELOG.rst index edf82b39d..a5acd427e 100644 --- a/ur_dashboard_msgs/CHANGELOG.rst +++ b/ur_dashboard_msgs/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package ur_dashboard_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.4.8 (2024-07-01) +------------------ + +2.4.7 (2024-06-19) +------------------ + +2.4.6 (2024-06-17) +------------------ + 2.4.5 (2024-05-16) ------------------ diff --git a/ur_dashboard_msgs/package.xml b/ur_dashboard_msgs/package.xml index 26f1b9acd..d62613278 100644 --- a/ur_dashboard_msgs/package.xml +++ b/ur_dashboard_msgs/package.xml @@ -2,7 +2,7 @@ ur_dashboard_msgs - 2.4.5 + 2.4.8 Messages around the UR Dashboard server. Felix Exner diff --git a/ur_moveit_config/CHANGELOG.rst b/ur_moveit_config/CHANGELOG.rst index b662833d2..d95d9942c 100644 --- a/ur_moveit_config/CHANGELOG.rst +++ b/ur_moveit_config/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package ur_moveit_config ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.4.8 (2024-07-01) +------------------ +* moveit_congig: Also install srdf folder (`#1033 `_) +* Contributors: Felix Exner (fexner) + +2.4.7 (2024-06-19) +------------------ + +2.4.6 (2024-06-17) +------------------ +* Make moveit_config compatible to moveit_configs_builder (`#998 `_) +* Contributors: Felix Exner (fexner), Vincenzo Di Pentima, Ruddick Lawrence + 2.4.5 (2024-05-16) ------------------ * Fix multi-line strings in DeclareLaunchArgument (`#948 `_) diff --git a/ur_moveit_config/CMakeLists.txt b/ur_moveit_config/CMakeLists.txt index df0cc0f94..80ff7c315 100644 --- a/ur_moveit_config/CMakeLists.txt +++ b/ur_moveit_config/CMakeLists.txt @@ -6,6 +6,6 @@ find_package(ament_cmake REQUIRED) ament_package() install( - DIRECTORY config launch + DIRECTORY config launch srdf DESTINATION share/${PROJECT_NAME} ) diff --git a/ur_moveit_config/package.xml b/ur_moveit_config/package.xml index 20c69437a..33e512651 100644 --- a/ur_moveit_config/package.xml +++ b/ur_moveit_config/package.xml @@ -2,7 +2,7 @@ ur_moveit_config - 2.4.5 + 2.4.8 An example package with MoveIt2 configurations for UR robots. diff --git a/ur_robot_driver/CHANGELOG.rst b/ur_robot_driver/CHANGELOG.rst index c7ffeb40b..a28cb3510 100644 --- a/ur_robot_driver/CHANGELOG.rst +++ b/ur_robot_driver/CHANGELOG.rst @@ -1,3 +1,22 @@ +2.4.8 (2024-07-01) +------------------ +* Add sleep between controller stopper's controller queries (`#1038 `_) +* Contributors: Felix Exner (fexner) + +2.4.7 (2024-06-19) +------------------ +* Fix launching without a tf_prefix specified (`#1029 `_) +* Contributors: Felix Exner (fexner) + +2.4.6 (2024-06-17) +------------------ +* Remove tf_prefix from ur_control.launch.py (`#1020 `_) +* Make moveit_config compatible to moveit_configs_builder (`#998 `_) +* Remove extra spaces from start_ursim statement in tests (`#1010 `_) +* Replace keepalive count (`#1002 `_) +* Restructure documentation for full stack documentation (`#984 `_) +* Contributors: Felix Exner, Ruddick Lawrence, Vincenzo Di Pentima + 2.4.5 (2024-05-16) ------------------ * Remove dependency to docker.io (`#985 `_) diff --git a/ur_robot_driver/doc/installation/installation.rst b/ur_robot_driver/doc/installation/installation.rst index b6820262a..9055669ad 100644 --- a/ur_robot_driver/doc/installation/installation.rst +++ b/ur_robot_driver/doc/installation/installation.rst @@ -4,6 +4,15 @@ Installation of the ur_robot_driver You can either install this driver from binary packages as shown above or build it from source. We recommend a binary package installation unless you want to join development and submit changes. +.. note:: + + Controlling the robot using ROS raises the requirement for strict cycle times. To achieve this, + we strongly recommend to use a lowlatency or even ``PREEMPT_RT``-patched kernel. See + :ref:`real time setup` for details on setting this up. + + For the same reason we encourage users to use a direct network connection between the ROS pc and + the robot controller without a switch. + Install from binary packages ---------------------------- diff --git a/ur_robot_driver/doc/installation/real_time.rst b/ur_robot_driver/doc/installation/real_time.rst deleted file mode 100644 index 2bb5b7869..000000000 --- a/ur_robot_driver/doc/installation/real_time.rst +++ /dev/null @@ -1,311 +0,0 @@ - -Setting up Ubuntu with a PREEMPT_RT kernel -========================================== - -In order to run the ``universal_robot_driver``\ , we highly recommend to setup a ubuntu system with -real-time capabilities. Especially with a robot from the e-Series the higher control frequency -might lead to non-smooth trajectory execution if not run using a real-time-enabled system. - -You might still be able to control the robot using a non-real-time system. This is, however, not recommended. - -To get real-time support into a ubuntu system, the following steps have to be performed: - - -#. Get the sources of a real-time kernel -#. Compile the real-time kernel -#. Setup user privileges to execute real-time tasks - -This guide will help you setup your system with a real-time kernel. - -Preparing ---------- - -To build the kernel, you will need a couple of tools available on your system. You can install them -using - -.. code-block:: bash - - $ sudo apt-get install build-essential bc ca-certificates gnupg2 libssl-dev wget gawk flex bison - -Before you download the sources of a real-time-enabled kernel, check the kernel version that is currently installed: - -.. code-block:: bash - - $ uname -r - 4.15.0-62-generic - -To continue with this tutorial, please create a temporary folder and navigate into it. You should -have sufficient space (around 25GB) there, as the extracted kernel sources take much space. After -the new kernel is installed, you can delete this folder again. - -In this example we will use a temporary folder inside our home folder: - -.. code-block:: bash - - $ mkdir -p ${HOME}/rt_kernel_build - $ cd ${HOME}/rt_kernel_build - -All future commands are expected to be run inside this folder. If the folder is different, the ``$`` -sign will be prefixed with a path relative to the above folder. - -Getting the sources for a real-time kernel ------------------------------------------- - -To build a real-time kernel, we first need to get the kernel sources and the real-time patch. - -First, we must decide on the kernel version that we want to use. Above, we -determined that our system has a 4.15 kernel installed. However, real-time -patches exist only for selected kernel versions. Those can be found on the -`linuxfoundation wiki `_. - -In this example, we will select a 4.14 kernel. Select a kernel version close to the -one installed on your system. - -Go ahead and download the kernel sources, patch sources and their signature files: - -.. code-block:: bash - - $ wget https://cdn.kernel.org/pub/linux/kernel/projects/rt/4.14/patch-4.14.139-rt66.patch.xz - $ wget https://cdn.kernel.org/pub/linux/kernel/projects/rt/4.14/patch-4.14.139-rt66.patch.sign - $ wget https://www.kernel.org/pub/linux/kernel/v4.x/linux-4.14.139.tar.xz - $ wget https://www.kernel.org/pub/linux/kernel/v4.x/linux-4.14.139.tar.sign - -To unzip the downloaded files do - -.. code-block:: bash - - $ xz -dk patch-4.14.139-rt66.patch.xz - $ xz -d linux-4.14.139.tar.xz - -Verification -^^^^^^^^^^^^ - -Technically, you can skip this section, it is however highly recommended to verify the file -integrity of such a core component of your system! - -To verify file integrity, you must first import public keys by the kernel developers and the patch -author. For the kernel sources use (as suggested on -`kernel.org `_\ ) - -.. code-block:: bash - - $ gpg2 --locate-keys torvalds@kernel.org gregkh@kernel.org - -and for the patch search for a key of the author listed on -`linuxfoundation wiki `_. - -.. code-block:: bash - - $ gpg2 --keyserver hkp://keys.gnupg.net --search-keys zanussi - gpg: data source: http://51.38.91.189:11371 - (1) German Daniel Zanussi - 4096 bit RSA key 0x537F98A9D92CEAC8, created: 2019-07-24, expires: 2023-07-24 - (2) Michael Zanussi - 4096 bit RSA key 0x7C7F76A2C1E3D9EB, created: 2019-05-08 - (3) Tom Zanussi - Tom Zanussi - Tom Zanussi - 4096 bit RSA key 0xDE09826778A38521, created: 2017-12-15 - (4) Riccardo Zanussi - 2048 bit RSA key 0xD299A06261D919C3, created: 2014-08-27, expires: 2018-08-27 (expired) - (5) Zanussi Gianni - 1024 bit DSA key 0x78B89CB020D1836C, created: 2004-04-06 - (6) Michael Zanussi - Michael Zanussi - Michael Zanussi - Michael Zanussi - 1024 bit DSA key 0xB3E952DCAC653064, created: 2000-09-05 - (7) Michael Zanussi - 1024 bit DSA key 0xEB10BBD9BA749318, created: 1999-05-31 - (8) Michael B. Zanussi - 1024 bit DSA key 0x39EE4EAD7BBB1E43, created: 1998-07-16 - Keys 1-8 of 8 for "zanussi". Enter number(s), N)ext, or Q)uit > 3 - -Now we can verify the downloaded sources: - -.. code-block:: bash - - $ gpg2 --verify linux-4.14.139.tar.sign - gpg: assuming signed data in 'linux-4.14.139.tar' - gpg: Signature made Fr 16 Aug 2019 10:15:17 CEST - gpg: using RSA key 647F28654894E3BD457199BE38DBBDC86092693E - gpg: Good signature from "Greg Kroah-Hartman " [unknown] - gpg: WARNING: This key is not certified with a trusted signature! - gpg: There is no indication that the signature belongs to the owner. - Primary key fingerprint: 647F 2865 4894 E3BD 4571 99BE 38DB BDC8 6092 693E - - $ gpg2 --verify patch-4.14.139-rt66.patch.sign - gpg: assuming signed data in 'patch-4.14.139-rt66.patch' - gpg: Signature made Fr 23 Aug 2019 21:09:20 CEST - gpg: using RSA key 0x0129F38552C38DF1 - gpg: Good signature from "Tom Zanussi " [unknown] - gpg: aka "Tom Zanussi " [unknown] - gpg: aka "Tom Zanussi " [unknown] - gpg: WARNING: This key is not certified with a trusted signature! - gpg: There is no indication that the signature belongs to the owner. - Primary key fingerprint: 5BDF C45C 2ECC 5387 D50C E5EF DE09 8267 78A3 8521 - Subkey fingerprint: ACF8 5F98 16A8 D5F0 96AE 1FD2 0129 F385 52C3 8DF1 - -Compilation ------------ - -Before we can compile the sources, we have to extract the tar archive and apply the patch - -.. code-block:: bash - - $ tar xf linux-4.14.139.tar - $ cd linux-4.14.139 - linux-4.14.139$ xzcat ../patch-4.14.139-rt66.patch.xz | patch -p1 - -Now to configure your kernel, just type - -.. code-block:: bash - - linux-4.14.139$ make oldconfig - -This will ask for kernel options. For everything else then the ``Preemption Model`` use the default -value (just press Enter) or adapt to your preferences. For the preemption model select ``Fully Preemptible Kernel``\ : - -.. code-block:: bash - - Preemption Model - 1. No Forced Preemption (Server) (PREEMPT_NONE) - > 2. Voluntary Kernel Preemption (Desktop) (PREEMPT_VOLUNTARY) - 3. Preemptible Kernel (Low-Latency Desktop) (PREEMPT__LL) (NEW) - 4. Preemptible Kernel (Basic RT) (PREEMPT_RTB) (NEW) - 5. Fully Preemptible Kernel (RT) (PREEMPT_RT_FULL) (NEW) - choice[1-5]: 5 - -Now you can build the kernel. This will take some time... - -.. code-block:: bash - - linux-4.14.139$ make -j $(getconf _NPROCESSORS_ONLN) deb-pkg - -After building, install the ``linux-headers`` and ``linux-image`` packages in the parent folder (only -the ones without the ``-dbg`` in the name) - -.. code-block:: bash - - linux-4.14.139$ sudo apt install ../linux-headers-4.14.139-rt66_*.deb ../linux-image-4.14.139-rt66_*.deb - -Setup user privileges to use real-time scheduling -------------------------------------------------- - -To be able to schedule threads with user privileges (what the driver will do) you'll have to change -the user's limits by changing ``/etc/security/limits.conf`` (See `the manpage `_ for details) - -We recommend to setup a group for real-time users instead of writing a fixed username into the config -file: - -.. code-block:: bash - - $ sudo groupadd realtime - $ sudo usermod -aG realtime $(whoami) - -Then, make sure ``/etc/security/limits.conf`` contains - -.. code-block:: - - @realtime soft rtprio 99 - @realtime soft priority 99 - @realtime soft memlock 102400 - @realtime hard rtprio 99 - @realtime hard priority 99 - @realtime hard memlock 102400 - -Note: You will have to log out and log back in (Not only close your terminal window) for these -changes to take effect. No need to do this now, as we will reboot later on, anyway. - -Setup GRUB to always boot the real-time kernel ----------------------------------------------- - -To make the new kernel the default kernel that the system will boot into every time, you'll have to -change the grub config file inside ``/etc/default/grub``. - -Note: This works for ubuntu, but might not be working for other linux systems. It might be necessary -to use another menuentry name there. - -But first, let's find out the name of the entry that we will want to make the default. You can list -all available kernels using - -.. code-block:: bash - - $ awk -F\' '/menuentry |submenu / {print $1 $2}' /boot/grub/grub.cfg - - menuentry Ubuntu - submenu Advanced options for Ubuntu - menuentry Ubuntu, with Linux 4.15.0-62-generic - menuentry Ubuntu, with Linux 4.15.0-62-generic (recovery mode) - menuentry Ubuntu, with Linux 4.15.0-60-generic - menuentry Ubuntu, with Linux 4.15.0-60-generic (recovery mode) - menuentry Ubuntu, with Linux 4.15.0-58-generic - menuentry Ubuntu, with Linux 4.15.0-58-generic (recovery mode) - menuentry Ubuntu, with Linux 4.14.139-rt66 - menuentry Ubuntu, with Linux 4.14.139-rt66 (recovery mode) - menuentry Memory test (memtest86+) - menuentry Memory test (memtest86+, serial console 115200) - menuentry Windows 7 (on /dev/sdc2) - menuentry Windows 7 (on /dev/sdc3) - -From the output above, we'll need to generate a string with the pattern ``"submenu_name>entry_name"``. In our case this would be - -.. code-block:: - - "Advanced options for Ubuntu>Ubuntu, with Linux 4.14.139-rt66" - -**The double quotes and no spaces around the ``>`` are important!** - -With this, we can setup the default grub entry and then update the grub menu entries. Don't forget this last step! - -.. code-block:: bash - - $ sudo sed -i 's/^GRUB_DEFAULT=.*/GRUB_DEFAULT="Advanced options for Ubuntu>Ubuntu, with Linux 4.14.139-rt66"/' /etc/default/grub - $ sudo update-grub - -Reboot the PC -------------- - -After having performed the above mentioned steps, reboot the PC. It should boot into the correct -kernel automatically. - -Check for preemption capabilities ---------------------------------- - -Make sure that the kernel does indeed support real-time scheduling: - -.. code-block:: bash - - $ uname -v | cut -d" " -f1-4 - #1 SMP PREEMPT RT - -Optional: Disable CPU speed scaling ------------------------------------ - -Many modern CPUs support changing their clock frequency dynamically depending on the currently -requested computation resources. In some cases this can lead to small interruptions in execution. -While the real-time scheduled controller thread should be unaffected by this, any external -components such as a visual servoing system might be interrupted for a short period on scaling -changes. - -To check and modify the power saving mode, install cpufrequtils: - -.. code-block:: bash - - $ sudo apt install cpufrequtils - -Run ``cpufreq-info`` to check available "governors" and the current CPU Frequency (\ ``current CPU -frequency is XXX MHZ``\ ). In the following we will set the governor to "performance". - -.. code-block:: bash - - $ sudo systemctl disable ondemand - $ sudo systemctl enable cpufrequtils - $ sudo sh -c 'echo "GOVERNOR=performance" > /etc/default/cpufrequtils' - $ sudo systemctl daemon-reload && sudo systemctl restart cpufrequtils - -This disables the ``ondemand`` CPU scaling daemon, creates a ``cpufrequtils`` config file and restarts -the ``cpufrequtils`` service. Check with ``cpufreq-info``. - -For further information about governors, please see the `kernel -documentation `_. diff --git a/ur_robot_driver/doc/installation/toc.rst b/ur_robot_driver/doc/installation/toc.rst index 2ee60aae8..a99fa5332 100644 --- a/ur_robot_driver/doc/installation/toc.rst +++ b/ur_robot_driver/doc/installation/toc.rst @@ -10,7 +10,6 @@ This chapter explains how to install the ``ur_robot_driver`` :caption: Contents: installation - real_time robot_setup install_urcap_cb3 install_urcap_e_series diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index e29269ce7..e7ca5c71d 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -68,6 +68,8 @@ def launch_setup(): parameters=[ LaunchConfiguration("update_rate_config_file"), ParameterFile(controllers_file, allow_substs=True), + # We use the tf_prefix as substitution in there, so that's why we keep it as an + # argument for this launchfile ], output="screen", ) diff --git a/ur_robot_driver/package.xml b/ur_robot_driver/package.xml index c01a02041..9f69f35be 100644 --- a/ur_robot_driver/package.xml +++ b/ur_robot_driver/package.xml @@ -2,7 +2,7 @@ ur_robot_driver - 2.4.5 + 2.4.8 The new driver for Universal Robots UR3, UR5 and UR10 robots with CB3 controllers and the e-series. Denis Stogl diff --git a/ur_robot_driver/src/controller_stopper.cpp b/ur_robot_driver/src/controller_stopper.cpp index 0a3ca2ec1..0ba15d881 100644 --- a/ur_robot_driver/src/controller_stopper.cpp +++ b/ur_robot_driver/src/controller_stopper.cpp @@ -42,6 +42,8 @@ #include #include +#include + #include "ur_robot_driver/controller_stopper.hpp" ControllerStopper::ControllerStopper(const rclcpp::Node::SharedPtr& node, bool stop_controllers_on_startup) @@ -90,6 +92,7 @@ ControllerStopper::ControllerStopper(const rclcpp::Node::SharedPtr& node, bool s } } } + rclcpp::sleep_for(std::chrono::milliseconds(100)); } auto request_switch_controller = std::make_shared(); request_switch_controller->strictness = request_switch_controller->STRICT; diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py index aaf59dabb..0bd4c5fa9 100644 --- a/ur_robot_driver/test/test_common.py +++ b/ur_robot_driver/test/test_common.py @@ -316,23 +316,26 @@ def generate_driver_test_description( ): ur_type = LaunchConfiguration("ur_type") + launch_arguments = { + "robot_ip": "192.168.56.101", + "ur_type": ur_type, + "launch_rviz": "false", + "controller_spawner_timeout": str(controller_spawner_timeout), + "initial_joint_controller": "scaled_joint_trajectory_controller", + "headless_mode": "true", + "launch_dashboard_client": "false", + "start_joint_controller": "false", + } + if tf_prefix: + launch_arguments["tf_prefix"] = tf_prefix + robot_driver = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( [FindPackageShare("ur_robot_driver"), "launch", "ur_control.launch.py"] ) ), - launch_arguments={ - "robot_ip": "192.168.56.101", - "ur_type": ur_type, - "launch_rviz": "false", - "controller_spawner_timeout": str(controller_spawner_timeout), - "initial_joint_controller": "scaled_joint_trajectory_controller", - "headless_mode": "true", - "launch_dashboard_client": "false", - "start_joint_controller": "false", - "tf_prefix": tf_prefix, - }.items(), + launch_arguments=launch_arguments.items(), ) wait_dashboard_server = ExecuteProcess( cmd=[