Started work on forward position controller

Creates errors on compilation due to multiple definition of funtions.

ur_controllers/CMakeLists.txt

@@ -54,10 +54,16 @@ generate_parameter_library(
   src/scaled_joint_trajectory_controller_parameters.yaml
 )
 
+generate_parameter_library(
+  forward_position_controller_parameters
+  src/forward_position_controller_parameters.yaml
+)
+
 add_library(${PROJECT_NAME} SHARED
   src/scaled_joint_trajectory_controller.cpp
   src/speed_scaling_state_broadcaster.cpp
-  src/gpio_controller.cpp)
+  src/gpio_controller.cpp
+  src/forward_position_controller.cpp)
 
 target_include_directories(${PROJECT_NAME} PRIVATE
   include
@@ -66,6 +72,7 @@ target_link_libraries(${PROJECT_NAME}
   gpio_controller_parameters
   speed_scaling_state_broadcaster_parameters
   scaled_joint_trajectory_controller_parameters
+  forward_position_controller_parameters
 )
 ament_target_dependencies(${PROJECT_NAME}
   ${THIS_PACKAGE_INCLUDE_DEPENDS}

ur_controllers/controller_plugins.xml

@@ -14,4 +14,9 @@
       This controller publishes the Tool IO.
     </description>
   </class>
+  <class name="ur_controllers/ForwardPositionController" type="ur_controllers::ForwardPositionController" base_class_type="controller_interface::ControllerInterface">
+    <description>
+      This controller forwards a cartesian trajectory to the robot controller for interpolation.
+    </description>
+  </class>
 </library>

ur_controllers/include/ur_controllers/forward_position_controller.hpp

@@ -0,0 +1,72 @@
+// Copyright 2024, Universal Robots A/S
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the {copyright_holder} nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Jacob Larsen [email protected]
+ * \date    2024-03-11
+ *
+ *
+ *
+ *
+ */
+//----------------------------------------------------------------------
+
+#ifndef UR_CONTROLLERS__FORWARD_POSITION_CONTROLLER_HPP_
+#define UR_CONTROLLERS__FORWARD_POSITION_CONTROLLER_HPP_
+
+#include <memory>
+// #include "controller_interface/controller_interface.hpp"
+#include "joint_trajectory_controller/joint_trajectory_controller.hpp"
+#include "forward_position_controller_parameters.hpp"
+
+namespace forward_controller
+{
+class ForwardPositionController : public joint_trajectory_controller::JointTrajectoryController
+{
+public:
+  ForwardPositionController() = default;
+  ~ForwardPositionController() override = default;
+
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State& state) override;
+
+  controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override;
+
+  controller_interface::CallbackReturn on_init() override;
+
+  void start_interpolation();
+
+private:
+  std::shared_ptr<forward_position_controller::ParamListener> forward_param_listener_;
+  forward_position_controller::Params forward_params_;
+};
+}  // namespace forward_controller
+#endif  // UR_CONTROLLERS__FORWARD_POSITION_CONTROLLER_HPP_

ur_controllers/src/forward_position_controller.cpp

@@ -0,0 +1,76 @@
+// Copyright 2024, Universal Robots A/S
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the {copyright_holder} nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Jacob Larsen [email protected]
+ * \date    2024-03-11
+ *
+ *
+ *
+ *
+ */
+//----------------------------------------------------------------------
+
+#include "ur_controllers/forward_position_controller.hpp"
+
+namespace forward_controller
+{
+controller_interface::CallbackReturn ForwardPositionController::on_init()
+{
+  forward_param_listener_ = std::make_shared<forward_position_controller::ParamListener>(get_node());
+  forward_params_ = forward_param_listener_->get_params();
+
+  return ControllerInterface::on_init();
+}
+
+controller_interface::InterfaceConfiguration ForwardPositionController::state_interface_configuration() const
+{
+  controller_interface::InterfaceConfiguration conf;
+
+  conf.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+
+  conf.names.push_back(forward_params_.speed_scaling_interface_name);
+
+  return conf;
+}
+
+controller_interface::CallbackReturn ForwardPositionController::on_activate(const rclcpp_lifecycle::State& state)
+{
+  return ControllerInterface::on_activate(state);
+}
+
+controller_interface::return_type ForwardPositionController::update(const rclcpp::Time& time,
+                                                                    const rclcpp::Duration& period)
+{
+  std::cout << state_interfaces_.back().get_value() << std::endl;
+  return controller_interface::return_type::OK;
+}
+
+}  // namespace forward_controller

ur_controllers/src/forward_position_controller_parameters.yaml

@@ -0,0 +1,6 @@
+forward_position_controller:
+    speed_scaling_interface_name: {
+      type: string,
+      default_value: "speed_scaling/speed_scaling_factor",
+      description: "Fully qualified name of the speed scaling interface name"
+    }

ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp

@@ -76,7 +76,8 @@ enum StoppingInterface
 {
   NONE,
   STOP_POSITION,
-  STOP_VELOCITY
+  STOP_VELOCITY,
+  STOP_FORWARD
 };
 
 /*!
@@ -220,6 +221,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
   std::vector<std::string> start_modes_;
   bool position_controller_running_;
   bool velocity_controller_running_;
+  bool forward_position_controller_running_;
 
   std::unique_ptr<urcl::UrDriver> ur_driver_;
   std::shared_ptr<std::thread> async_thread_;

ur_robot_driver/src/hardware_interface.cpp

@@ -85,6 +85,7 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys
   start_modes_ = {};
   position_controller_running_ = false;
   velocity_controller_running_ = false;
+  forward_position_controller_running_ = false;
   runtime_state_ = static_cast<uint32_t>(rtde::RUNTIME_STATE::STOPPED);
   pausing_state_ = PausingState::RUNNING;
   pausing_ramp_up_increment_ = 0.01;
@@ -632,6 +633,8 @@ hardware_interface::return_type URPositionHardwareInterface::write(const rclcpp:
     } else if (velocity_controller_running_) {
       ur_driver_->writeJointCommand(urcl_velocity_commands_, urcl::comm::ControlMode::MODE_SPEEDJ);
 
+    } else if (forward_position_controller_running_) {
+      ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP);
     } else {
       ur_driver_->writeKeepalive();
     }