Write DOUT 0-7 to CO8-CO15 on a CB7 (backport #2047)

ur_robot_driver/doc/usage/utility_controllers.rst

@@ -0,0 +1,172 @@
+:github_url: https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/blob/main/ur_robot_driver/doc/usage/utility_controllers.rst
+
+.. _utility_controllers:
+
+Utility Controllers
+===================
+
+These controllers provide access to UR robot-specific features that complement the motion control
+modes. They are not motion controllers themselves but provide status information, safety features,
+and special robot modes.
+
+For position and velocity based control, see :ref:`position_velocity_control`.
+
+For force and torque based control, see :ref:`force_torque_control`.
+
+
+joint_state_broadcaster
+-----------------------
+
+Type: `joint_state_broadcaster/JointStateBroadcaster <https://control.ros.org/rolling/doc/ros2_controllers/joint_state_broadcaster/doc/userdoc.html>`_
+
+Publishes all joints' positions, velocities, and efforts as ``sensor_msgs/JointState`` on the
+``joint_states`` topic. This broadcaster is read-only and can run alongside any other controller.
+
+.. note::
+
+   On older ROS distributions, the effort interfaces reported motor currents instead of actual
+   torques. This is no longer the case, and the driver now reports actual joint torques by
+   default, which requires PolyScope >= 5.23.0 / 10.11.0. If you are using an older robot
+   software version, the driver will fail to start and print an error about the missing RTDE
+   variable ``actual_current_as_torque``. To revert to the previous behavior, set the hardware
+   parameter ``use_currents_as_efforts`` to ``true``. See
+   :ref:`hardware_interface_parameters` for details.
+
+speed_scaling_state_broadcaster
+-------------------------------
+
+Type: :ref:`ur_controllers/SpeedScalingStateBroadcaster <speed_scaling_state_broadcaster>`
+
+Publishes the current actual execution speed as reported by the robot. Values are floating points
+between 0 and 1. This broadcaster is read-only and can run alongside any other controller.
+
+The speed scaling value reflects the combined effect of the speed slider on the teach pendant and
+any controller-imposed speed limits. It is used by the ``joint_trajectory_controller`` to scale
+trajectory execution on the ROS PC. For the ``passthrough_trajectory_controller`` and the
+``motion_primitive_forward_controller``, speed scaling is handled directly by the robot controller
+since it performs the interpolation in these modes.
+
+io_and_status_controller
+------------------------
+
+Type: :ref:`ur_controllers/GPIOController <io_and_status_controller>`
+
+Allows setting I/O ports, controlling some UR-specific functionality, and publishes status
+information about the robot. This controller is always active.
+
+Key services include:
+
+* ``~/set_io``: Set digital output pins.
+* ``~/set_payload``: Change the robot's payload on-the-fly.
+* ``~/set_speed_slider``: Set the speed slider value.
+* ``~/zero_ftsensor``: Zero the force/torque sensor.
+* ``~/resend_robot_program``: Restart the external control program (:ref:`headless mode <headless_mode>`).
+
+See the :ref:`io_and_status_controller <io_and_status_controller>` documentation for the full list
+of topics and services.
+
+.. note::
+
+   On **CB7** robots, the (non-configurable) digital outputs have been transformed to configurable
+   outputs. The driver uses a concatenated list for both, unconfigurable and configurable outputs,
+   where indices 8-15 refer to the configurable outputs CO0-CO7. This mapping has been kept for CB7
+   robots. Hence, CO8-CO15 are mapped to indices 0-7. This affects both, the ``~/set_io`` service
+   and the ``~/io_states`` topic.
+
+   The following table shows the mapping for both configurations:
+
+   +----------------------------+-----------------------+----------------------------+-----------------------------+
+   | index mapping in io states | digital_outputs (0-7) | configurable_outputs (0-7) | configurable_outputs (8-15) |
+   +============================+=======================+============================+=============================+
+   | CB3, CB5                   | 0-7                   | 8-15                       | n/a                         |
+   +----------------------------+-----------------------+----------------------------+-----------------------------+
+   | CB7                        | n/a                   | 8-15                       | 0-7                         |
+   +----------------------------+-----------------------+----------------------------+-----------------------------+
+
+tool_contact_controller
+-----------------------
+
+Type: :ref:`ur_controllers/ToolContactController <tool_contact_controller>`
+
+Enables the robot's tool contact detection. When tool contact is detected, the robot stops all
+motion and retracts to where it first sensed the contact. This is useful for probing, surface
+detection, and safe approach applications.
+
+The controller works with the ``joint_trajectory_controller`` and the
+``passthrough_trajectory_controller``.
+
+The controller provides an action interface:
+
+* ``~/detect_tool_contact [ur_msgs/action/ToolContact]``
+
+.. code-block:: console
+
+   $ ros2 action send_goal /tool_contact_controller/detect_tool_contact \
+     ur_msgs/action/ToolContact
+
+See the :ref:`tool_contact_controller <tool_contact_controller>` documentation for full details.
+
+
+tcp_pose_broadcaster
+--------------------
+
+Type: `pose_broadcaster/PoseBroadcaster <https://control.ros.org/rolling/doc/ros2_controllers/pose_broadcaster/doc/userdoc.html>`_
+
+Publishes the robot's TCP pose. This broadcaster is read-only and can run alongside any other
+controller.
+
+The robot's TCP pose is published both as a ``geometry_msgs/PoseStamped`` on the ``tcp_pose_broadcaster/pose`` topic and as a
+``tf2`` transform with the frame name ``tool0_controller`` with ``base`` as a
+parent.
+
+Thus, the transformation from ``base`` to ``tool0_controller`` doesn't use the URDF model and
+forward kinematics in ROS, but instead directly uses the robot's internal kinematics and sensors
+(which will always use the robot's calibration).
+
+.. note::
+   When setting a tool on the robot's teach pendant, this will affect the tcp pose as published by
+   this controller, as the robot will take the tool's geometry into account when calculating the
+   TCP pose.
+
+   Setting up the :ref:`URDF with the robot's calibration <ur_calibration>` will make the ``tool0``
+   and ``tool0_controller`` frames align exactly given no tool is setup on the robot.
+
+   All frames mentioned will be prefixed with the robot's ``tf_prefix`` if it is set. It has been
+   omitted here for readability.
+
+.. figure:: tcp_frames.png
+   :align: center
+   :alt: tcp frames (tool0 and tool0_controller)
+
+   The ``tool0_controller`` frame is published with ``base`` as parent directly, while
+   ``tool0`` is attached to the URDF model chain. When a TCP is setup on the robot, the
+   ``tool0_controller`` frame will take that into account (as on the right, where the tool has a
+   z-offset of 110 mm).
+
+ur_configuration_controller
+----------------------------
+
+Type: :ref:`ur_controllers/URConfigurationController <ur_configuration_controller>`
+
+Provides access to UR-specific robot configuration data. This controller is always active.
+
+gravity_update_controller
+-------------------------
+
+Type: :ref:`ur_controllers/GravityUpdateController <gravity_update_controller>`
+
+Updates the gravity vector used by the robot for gravity compensation. This is useful when the
+robot is mounted in a non-standard orientation, such as on a wall or ceiling.
+
+The controller exposes the ``~/set_gravity`` service
+(`ur_msgs/srv/SetGravity <https://docs.ros.org/en/rolling/p/ur_msgs/srv/SetGravity.html>`_) to
+set the gravity direction. The vector in the request points towards the Earth's center and can be
+expressed in any frame transformable to the robot's ``base`` frame. It is started as active by
+default in the driver's launch file.
+
+.. code-block:: console
+
+   $ ros2 service call /gravity_update_controller/set_gravity ur_msgs/srv/SetGravity \
+     "{gravity: {header: {frame_id: 'base'}, vector: {x: 0.0, y: 0.0, z: -9.82}}}"
+
+See the :ref:`gravity_update_controller <gravity_update_controller>` documentation for full details.

ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp

@@ -357,6 +357,8 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
   std::function<bool()> get_data_package;
 
   bool use_currents_as_efforts_ = true;
+
+  urcl::ControlBoxType control_box_type_ = urcl::ControlBoxType::UNKNOWN;
 };
 }  // namespace ur_robot_driver
 

ur_robot_driver/src/hardware_interface.cpp

@@ -778,6 +778,8 @@ URPositionHardwareInterface::on_configure(const rclcpp_lifecycle::State& previou
     }
   }
 
+  control_box_type_ = ur_driver_->getPrimaryClient()->getControlBoxType();
+
   RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Calibration checksum: '%s'.",
               calibration_checksum.c_str());
   // check calibration
@@ -1091,8 +1093,13 @@ void URPositionHardwareInterface::checkAsyncIO()
   for (size_t i = 0; i < 18; ++i) {
     if (!std::isnan(standard_dig_out_bits_cmd_[i]) && ur_driver_ != nullptr) {
       if (i <= 7) {
-        io_async_success_ =
-            ur_driver_->getRTDEWriter().sendStandardDigitalOutput(i, static_cast<bool>(standard_dig_out_bits_cmd_[i]));
+        if (control_box_type_ == urcl::ControlBoxType::CB7) {
+          io_async_success_ = ur_driver_->getRTDEWriter().sendConfigurableDigitalOutput(
+              static_cast<uint8_t>(i + 8), static_cast<bool>(standard_dig_out_bits_cmd_[i]));
+        } else {
+          io_async_success_ = ur_driver_->getRTDEWriter().sendStandardDigitalOutput(
+              i, static_cast<bool>(standard_dig_out_bits_cmd_[i]));
+        }
       } else if (i <= 15) {
         io_async_success_ = ur_driver_->getRTDEWriter().sendConfigurableDigitalOutput(
             static_cast<uint8_t>(i - 8), static_cast<bool>(standard_dig_out_bits_cmd_[i]));