From 72a5ed5fdb933901376684804fbf5c1a1e1c0d01 Mon Sep 17 00:00:00 2001 From: Mads Holm Peters Date: Wed, 28 May 2025 15:49:02 +0200 Subject: [PATCH 1/3] Added two new control modes PD controller in joint space and PD controller in task space The pd controller in joint space will accept joint targets and compute joint torques based on PD control scheme. The pd controller in task space will accept task space targets and use inverse kinematics to compute joint targets which are then used to compute joint torques based on PD control scheme. --- examples/CMakeLists.txt | 4 + examples/pd_controller_example.cpp | 241 ++++++++++++++++++ include/ur_client_library/comm/control_mode.h | 15 +- .../torque_command_controller_parameters.h | 97 +++++++ resources/external_control.urscript | 54 ++++ src/ur/ur_driver.cpp | 33 +++ tests/test_reverse_interface.cpp | 18 ++ 7 files changed, 457 insertions(+), 5 deletions(-) create mode 100644 examples/pd_controller_example.cpp create mode 100644 include/ur_client_library/control/torque_command_controller_parameters.h diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index e27ea26d..f13be169 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -55,3 +55,7 @@ target_link_libraries(trajectory_point_interface_example ur_client_library::urcl add_executable(instruction_executor instruction_executor.cpp) target_link_libraries(instruction_executor ur_client_library::urcl) + +add_executable(pd_controller_example +pd_controller_example.cpp) +target_link_libraries(pd_controller_example ur_client_library::urcl) diff --git a/examples/pd_controller_example.cpp b/examples/pd_controller_example.cpp new file mode 100644 index 00000000..a959a4e7 --- /dev/null +++ b/examples/pd_controller_example.cpp @@ -0,0 +1,241 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2025 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. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include "ur_client_library/example_robot_wrapper.h" +#include "ur_client_library/ur/dashboard_client.h" +#include "ur_client_library/ur/ur_driver.h" +#include "ur_client_library/types.h" +#include "ur_client_library/ur/instruction_executor.h" + +#include +#include +#include +#include + +using namespace urcl; + +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"; + +std::unique_ptr g_my_robot; + +void signal_callback_handler(int signum) +{ + std::cout << "Caught signal " << signum << std::endl; + if (g_my_robot != nullptr) + { + // Stop control of the robot + g_my_robot->getUrDriver()->stopControl(); + } + // Terminate program + exit(signum); +} + +struct DataStorage +{ + std::vector target; + std::vector actual; + std::vector time; + + void write_to_file(const std::string& filename) + { + std::fstream output(filename, std::ios::out); + output << "timestamp,target0,target1,target2,target3,target4,target5,actual0,actual1,actual2," + "actual3,actual4,actual5\n"; + for (size_t i = 0; i < time.size(); ++i) + { + output << time[i]; + for (auto& t : target[i]) + { + output << "," << t; + } + for (auto& a : actual[i]) + { + output << "," << a; + } + output << "\n"; + } + output.close(); + } +}; + +bool pd_control_loop(DataStorage& data_storage, const std::string& actual_data_name, + const comm::ControlMode control_mode, const urcl::vector6d_t& amplitude) +{ + const int32_t running_time = 13; + double time = 0.0; + + // Reserve space for expected amount of data + data_storage.actual.reserve(running_time * 500); + data_storage.target.reserve(running_time * 500); + data_storage.time.reserve(running_time * 500); + + urcl::vector6d_t actual, target, start = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + + bool first_pass = true; + while (time < running_time) + { + const auto t_start = std::chrono::high_resolution_clock::now(); + // 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_robot->getUrDriver()->getDataPackage(); + if (!data_pkg) + { + URCL_LOG_WARN("Could not get fresh data package from robot"); + return false; + } + // Read current joint positions from robot data + if (!data_pkg->getData(actual_data_name, actual)) + { + // This throwing should never happen unless misconfigured + std::string error_msg = "Did not find" + actual_data_name + "in data sent from robot. This should not happen!"; + throw std::runtime_error(error_msg); + } + + if (first_pass) + { + target = actual; + start = actual; + for (size_t i = 0; i < start.size(); ++i) + { + start[i] = start[i] - amplitude[i]; + } + first_pass = false; + } + + for (size_t i = 0; i < target.size(); ++i) + { + target[i] = start[i] + amplitude[i] * cos(time); + } + + // 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_robot->getUrDriver()->writeJointCommand(target, control_mode, RobotReceiveTimeout::millisec(100)); + if (!ret) + { + URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?"); + return false; + } + + // Increment time and log data + const auto t_end = std::chrono::high_resolution_clock::now(); + const double elapsed_time_ms = std::chrono::duration(t_end - t_start).count() / 1000; + time = time + elapsed_time_ms; + data_storage.actual.push_back(actual); + data_storage.target.push_back(target); + data_storage.time.push_back(time); + } + + return true; +} + +int main(int argc, char* argv[]) +{ + // This will make sure that we stop controlling the robot if the user presses ctrl-c + signal(SIGINT, signal_callback_handler); + + urcl::setLogLevel(urcl::LogLevel::INFO); + + // Parse the ip arguments if given + std::string robot_ip = DEFAULT_ROBOT_IP; + if (argc > 1) + { + robot_ip = std::string(argv[1]); + } + + const bool headless_mode = true; + g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode); + + if (!g_my_robot->isHealthy()) + { + URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above."); + return 1; + } + + auto instruction_executor = std::make_shared(g_my_robot->getUrDriver()); + + URCL_LOG_INFO("Move the robot to initial position"); + instruction_executor->moveJ(urcl::vector6d_t{ 0, -1.67, -0.65, -1.59, 1.61, 5.09 }, 0.5, 0.2, 5); + + DataStorage joint_controller_storage; + + // 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_robot->getUrDriver()->startRTDECommunication(); + URCL_LOG_INFO("Start controlling the robot using the PD controller"); + const bool completed_joint_control = + pd_control_loop(joint_controller_storage, "actual_q", comm::ControlMode::MODE_PD_CONTROLLER_JOINT, + { 0.2, 0.2, 0.2, 0.2, 0.2, 0.2 }); + if (!completed_joint_control) + { + URCL_LOG_ERROR("Didn't complete pd control in joint space"); + g_my_robot->getUrDriver()->stopControl(); + return 1; + } + + // Read the rtde packages in the background while moving to initial position for PD control in task space + g_my_robot->startConsumingRTDEData(); + + URCL_LOG_INFO("Move the robot to initial position for task space PD control"); + instruction_executor->moveJ(urcl::vector6d_t{ 0.03, -0.98, -1.83, -1.70, 1.37, 5.12 }, 0.5, 0.2, 5); + + DataStorage task_controller_storage; + + g_my_robot->stopConsumingRTDEData(); + URCL_LOG_INFO("Start controlling the robot using the PD controller with task space targets"); + const bool completed_task_control = + pd_control_loop(task_controller_storage, "actual_TCP_pose", comm::ControlMode::MODE_PD_CONTROLLER_TASK, + { 0.05, 0.05, 0.05, 0.02, 0.02, 0.02 }); + if (!completed_task_control) + { + URCL_LOG_ERROR("Didn't complete PD control in task space"); + g_my_robot->getUrDriver()->stopControl(); + return 1; + } + + // Consume data in the background while writing the logged data to a file + g_my_robot->startConsumingRTDEData(); + g_my_robot->getUrDriver()->stopControl(); + URCL_LOG_INFO("Movement done"); + + // Write data to files, can be used for benchmarking the controller (commented out as default) + // joint_controller_storage.write_to_file("examples/pd_controller_joint.csv"); + // task_controller_storage.write_to_file("examples/pd_controller_task.csv"); + g_my_robot->stopConsumingRTDEData(); + + return 0; +} diff --git a/include/ur_client_library/comm/control_mode.h b/include/ur_client_library/comm/control_mode.h index 51156da6..f1da507a 100644 --- a/include/ur_client_library/comm/control_mode.h +++ b/include/ur_client_library/comm/control_mode.h @@ -53,7 +53,9 @@ enum class ControlMode : int32_t MODE_TOOL_IN_CONTACT = 7, ///< Used only internally in the script, when robot is in tool contact, clear by endToolContact() MODE_TORQUE = 8, ///< Set when torque control is active. - END ///< This is not an actual control mode, but used internally to get the number of control modes + MODE_PD_CONTROLLER_JOINT = 9, ///< Set when PD control in joint space is active + MODE_PD_CONTROLLER_TASK = 10, ///< Set when PD control in task space is active + END ///< This is not an actual control mode, but used internally to get the number of control modes }; /*! @@ -63,10 +65,13 @@ class ControlModeTypes { public: // Control modes that require realtime communication - static const inline std::vector REALTIME_CONTROL_MODES = { - ControlMode::MODE_SERVOJ, ControlMode::MODE_SPEEDJ, ControlMode::MODE_SPEEDL, ControlMode::MODE_POSE, - ControlMode::MODE_TORQUE - }; + static const inline std::vector REALTIME_CONTROL_MODES = { ControlMode::MODE_SERVOJ, + ControlMode::MODE_SPEEDJ, + ControlMode::MODE_SPEEDL, + ControlMode::MODE_POSE, + ControlMode::MODE_TORQUE, + ControlMode::MODE_PD_CONTROLLER_JOINT, + ControlMode::MODE_PD_CONTROLLER_TASK }; // Control modes that doesn't require realtime communication static const inline std::vector NON_REALTIME_CONTROL_MODES = { ControlMode::MODE_IDLE, diff --git a/include/ur_client_library/control/torque_command_controller_parameters.h b/include/ur_client_library/control/torque_command_controller_parameters.h new file mode 100644 index 00000000..62017cf9 --- /dev/null +++ b/include/ur_client_library/control/torque_command_controller_parameters.h @@ -0,0 +1,97 @@ +#include "ur_client_library/ur/datatypes.h" +#include "ur_client_library/log.h" + +#include + +namespace urcl +{ +namespace control +{ + +struct PDControllerGains +{ + vector6d_t kp; + vector6d_t kd; +}; + +// UR3 PD controller gains, needs to be tuned for the specific purpose. +constexpr PDControllerGains UR3_PD_CONTROLLER_PARAMETERS_JOINT_SPACE{ + /*.kp*/ { 560.0, 560.0, 350.0, 163.0, 163.0, 163.0 }, + /*.kd*/ { 47.32, 47.32, 37.42, 25.5, 25.5, 25.5 } +}; + +// UR5 PD controller gains, needs to be tuned for the specific purpose. +constexpr PDControllerGains UR5_PD_CONTROLLER_PARAMETERS_JOINT_SPACE{ + /*.kp*/ { 900.0, 900.0, 900.0, 500.0, 500.0, 500.0 }, + /*.kd*/ { 60.0, 60.0, 60.0, 44.72, 44.72, 44.72 } +}; + +// UR10 PD controller gains, needs to be tuned for the specific purpose. +constexpr PDControllerGains UR10_PD_CONTROLLER_PARAMETERS_JOINT_SPACE{ + /*.kp*/ { 1300.0, 1300.0, 900.0, 560.0, 560.0, 560.0 }, + /*.kd*/ { 72.11, 72.11, 60.0, 47.32, 47.32, 47.32 } +}; + +constexpr vector6d_t MAX_UR3_JOINT_TORQUE = { 54.0, 54.0, 28.0, 9.0, 9.0, 9.0 }; + +constexpr vector6d_t MAX_UR5_JOINT_TORQUE = { 150.0, 150.0, 150.0, 28.0, 28.0, 28.0 }; + +constexpr vector6d_t MAX_UR10_JOINT_TORQUE = { 330.0, 330.0, 150.0, 54.0, 54.0, 54.0 }; + +/*! + * \brief Get pd gains for specific robot type + * + * \param robot_type Robot type to get gains for + * + * \returns PD gains for the specific robot type + */ +inline PDControllerGains getPdGainsFromRobotType(RobotType robot_type) +{ + switch (robot_type) + { + case RobotType::UR3: + return UR3_PD_CONTROLLER_PARAMETERS_JOINT_SPACE; + case RobotType::UR5: + return UR5_PD_CONTROLLER_PARAMETERS_JOINT_SPACE; + case RobotType::UR10: + return UR10_PD_CONTROLLER_PARAMETERS_JOINT_SPACE; + default: + std::stringstream ss; + ss << "Default gains has not been tuned for robot type " << static_cast(robot_type) + << ". Defaulting to UR10 gains."; + URCL_LOG_WARN(ss.str().c_str()); + return UR10_PD_CONTROLLER_PARAMETERS_JOINT_SPACE; + } +} + +/*! + * \brief Get max torques specific robot type + * + * \param robot_type Robot type to get max torque for + * + * \returns max torque for the specific robot type + */ +inline vector6d_t getMaxTorquesFromRobotType(RobotType robot_type) +{ + switch (robot_type) + { + case RobotType::UR3: + return MAX_UR3_JOINT_TORQUE; + case RobotType::UR5: + return MAX_UR5_JOINT_TORQUE; + case RobotType::UR10: + return MAX_UR10_JOINT_TORQUE; + case RobotType::UR16: + // Same joints as ur10 + return MAX_UR10_JOINT_TORQUE; + default: + std::stringstream ss; + ss << "Max joint torques not collected for robot type " << static_cast(robot_type) + << ". Defaulting to UR10 max joint torques."; + URCL_LOG_WARN(ss.str().c_str()); + return MAX_UR10_JOINT_TORQUE; + } +} + +} // namespace control +} // namespace urcl \ No newline at end of file diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 181e9e61..b848f2ae 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -26,6 +26,8 @@ MODE_POSE = 5 MODE_FREEDRIVE = 6 MODE_TOOL_IN_CONTACT = 7 MODE_TORQUE = 8 +MODE_PD_CONTROLLER_JOINT = 9 +MODE_PD_CONTROLLER_TASK = 10 # Data dimensions of the message received on the reverse interface REVERSE_INTERFACE_DATA_DIMENSION = 8 @@ -85,6 +87,8 @@ global spline_qd = [0, 0, 0, 0, 0, 0] global tool_contact_running = False global trajectory_result = 0 global friction_compensation_enabled = True +global pd_controller_gains = {{PD_CONTROLLER_GAINS_REPLACE}} +global max_joint_torques = {{MAX_JOINT_TORQUE_REPLACE}} # Global thread variables thread_move = 0 @@ -154,6 +158,33 @@ def terminateProgram(): halt end +### +# @brief Function to clamp each element of a list in between +- clamp value. +# +# @param values array is the list of values to clamp +# @param clampvalues array is the list of clamp values +# +# @returns array of values within +- clamp value +### +def clamp_array(values, clampValues): + if length(values) != length(clampValues): + popup("List of values has different length than list of clamp values.", error = True, blocking = True) + end + pdiff = values - clampValues + mdiff = values + clampValues + ret = values + j = 0 + while j < length(values): + if pdiff[j] > 0: + ret[j] = clampValues[j] + elif mdiff[j] < 0: + ret[j] = -clampValues[j] + end + j = j + 1 + end + return ret +end + def set_servo_setpoint(q): cmd_servo_state = SERVO_RUNNING if targetWithinLimits(cmd_servo_q, q, steptime): @@ -644,6 +675,17 @@ thread servoThreadP(): stopj(STOPJ_ACCELERATION) end +thread PDControlThread(): + while control_mode == MODE_PD_CONTROLLER_JOINT or control_mode == MODE_PD_CONTROLLER_TASK: + local q_err = cmd_servo_q - get_actual_joint_positions() + local tau = pd_controller_gains.kp * q_err - pd_controller_gains.kd * get_actual_joint_speeds() + tau = clamp_array(tau, max_joint_torques) + torque_command(tau, friction_comp=friction_compensation_enabled) + end + textmsg("PD Control thread ended") + stopj(STOPJ_ACCELERATION) +end + def tool_contact_detection(): # Detect tool contact in the directions that the TCP is moving step_back = tool_contact(direction = get_actual_tcp_speed()) @@ -784,6 +826,12 @@ while control_mode > MODE_STOPPED: elif control_mode == MODE_POSE: cmd_servo_q = get_joint_positions() thread_move = run servoThreadP() + elif control_mode == MODE_PD_CONTROLLER_JOINT: + cmd_servo_q = get_joint_positions() + thread_move = run PDControlThread() + elif control_mode == MODE_PD_CONTROLLER_TASK: + cmd_servo_q = get_joint_positions() + thread_move = run PDControlThread() end end @@ -824,6 +872,12 @@ while control_mode > MODE_STOPPED: textmsg("Leaving freedrive mode") end_freedrive_mode() end + elif control_mode == MODE_PD_CONTROLLER_JOINT: + q = [params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate, params_mult[7] / MULT_jointstate] + set_servo_setpoint(q) + elif control_mode == MODE_PD_CONTROLLER_TASK: + pose = p[params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate, params_mult[7] / MULT_jointstate] + set_servo_setpoint(get_inverse_kin(pose, cmd_servo_q)) end # Tool contact is running, but hasn't been detected if tool_contact_running == True and control_mode != MODE_TOOL_IN_CONTACT: diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 6e30579c..438f3fb1 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -34,6 +34,8 @@ #include "ur_client_library/ur/ur_driver.h" #include "ur_client_library/exceptions.h" #include "ur_client_library/primary/primary_parser.h" +#include "ur_client_library/control/torque_command_controller_parameters.h" +#include "ur_client_library/helpers.h" #include #include @@ -51,6 +53,8 @@ static const std::string TRAJECTORY_PORT_REPLACE("{{TRAJECTORY_SERVER_PORT_REPLA static const std::string SCRIPT_COMMAND_PORT_REPLACE("{{SCRIPT_COMMAND_SERVER_PORT_REPLACE}}"); static const std::string FORCE_MODE_SET_DAMPING_REPLACE("{{FORCE_MODE_SET_DAMPING_REPLACE}}"); static const std::string FORCE_MODE_SET_GAIN_SCALING_REPLACE("{{FORCE_MODE_SET_GAIN_SCALING_REPLACE}}"); +static const std::string PD_CONTROLLER_GAINS_REPLACE("{{PD_CONTROLLER_GAINS_REPLACE}}"); +static const std::string MAX_JOINT_TORQUE_REPLACE("{{MAX_JOINT_TORQUE_REPLACE}}"); void UrDriver::init(const UrDriverConfiguration& config) { @@ -146,6 +150,35 @@ void UrDriver::init(const UrDriverConfiguration& config) script_command_interface_.reset(new control::ScriptCommandInterface(config.script_command_port)); startPrimaryClientCommunication(); + + std::chrono::milliseconds timeout(1000); + try + { + waitFor([this]() { return primary_client_->getConfigurationData() != nullptr; }, timeout); + } + catch (const TimeoutException&) + { + throw TimeoutException("Could not get configuration package within timeout, are you connected to the robot?", + timeout); + } + + const RobotType robot_type = primary_client_->getRobotType(); + + if (prog.find(PD_CONTROLLER_GAINS_REPLACE) != std::string::npos) + { + const control::PDControllerGains pd_gains = control::getPdGainsFromRobotType(robot_type); + std::stringstream ss; + ss << "struct(kp=" << pd_gains.kp << ", kd=" << pd_gains.kd << ")"; + prog.replace(prog.find(PD_CONTROLLER_GAINS_REPLACE), PD_CONTROLLER_GAINS_REPLACE.length(), ss.str()); + } + + if (prog.find(MAX_JOINT_TORQUE_REPLACE) != std::string::npos) + { + std::stringstream ss; + ss << control::getMaxTorquesFromRobotType(robot_type); + prog.replace(prog.find(MAX_JOINT_TORQUE_REPLACE), MAX_JOINT_TORQUE_REPLACE.length(), ss.str()); + } + if (in_headless_mode_) { full_robot_program_ = "stop program\n"; diff --git a/tests/test_reverse_interface.cpp b/tests/test_reverse_interface.cpp index 73466cfd..af05b2dc 100644 --- a/tests/test_reverse_interface.cpp +++ b/tests/test_reverse_interface.cpp @@ -395,6 +395,24 @@ TEST_F(ReverseIntefaceTest, write_control_mode) expected_control_mode = comm::ControlMode::MODE_UNINITIALIZED; EXPECT_THROW(reverse_interface_->write(&pos, expected_control_mode), UrException); + + expected_control_mode = comm::ControlMode::MODE_TORQUE; + reverse_interface_->write(&pos, expected_control_mode); + received_control_mode = client_->getControlMode(); + + EXPECT_EQ(toUnderlying(expected_control_mode), received_control_mode); + + expected_control_mode = comm::ControlMode::MODE_PD_CONTROLLER_JOINT; + reverse_interface_->write(&pos, expected_control_mode); + received_control_mode = client_->getControlMode(); + + EXPECT_EQ(toUnderlying(expected_control_mode), received_control_mode); + + expected_control_mode = comm::ControlMode::MODE_PD_CONTROLLER_TASK; + reverse_interface_->write(&pos, expected_control_mode); + received_control_mode = client_->getControlMode(); + + EXPECT_EQ(toUnderlying(expected_control_mode), received_control_mode); } TEST_F(ReverseIntefaceTest, write_freedrive_control_message) From e229cd2d3af4166f4fdefe6315e35963bd1fb2f6 Mon Sep 17 00:00:00 2001 From: Mads Holm Peters <79145214+urmahp@users.noreply.github.com> Date: Thu, 5 Jun 2025 08:27:50 +0200 Subject: [PATCH 2/3] Apply suggestions from code review Co-authored-by: Felix Exner --- .../control/torque_command_controller_parameters.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/ur_client_library/control/torque_command_controller_parameters.h b/include/ur_client_library/control/torque_command_controller_parameters.h index 62017cf9..5a4a7be9 100644 --- a/include/ur_client_library/control/torque_command_controller_parameters.h +++ b/include/ur_client_library/control/torque_command_controller_parameters.h @@ -57,7 +57,7 @@ inline PDControllerGains getPdGainsFromRobotType(RobotType robot_type) return UR10_PD_CONTROLLER_PARAMETERS_JOINT_SPACE; default: std::stringstream ss; - ss << "Default gains has not been tuned for robot type " << static_cast(robot_type) + ss << "Default gains has not been tuned for robot type " << robotTypeString(robot_type) << ". Defaulting to UR10 gains."; URCL_LOG_WARN(ss.str().c_str()); return UR10_PD_CONTROLLER_PARAMETERS_JOINT_SPACE; @@ -86,7 +86,7 @@ inline vector6d_t getMaxTorquesFromRobotType(RobotType robot_type) return MAX_UR10_JOINT_TORQUE; default: std::stringstream ss; - ss << "Max joint torques not collected for robot type " << static_cast(robot_type) + ss << "Max joint torques not collected for robot type " << robotTypeString(robot_type) << ". Defaulting to UR10 max joint torques."; URCL_LOG_WARN(ss.str().c_str()); return MAX_UR10_JOINT_TORQUE; From c5833ed0cc8bdea597d7f6fccb26480136e6c724 Mon Sep 17 00:00:00 2001 From: Mads Holm Peters Date: Thu, 5 Jun 2025 08:34:58 +0200 Subject: [PATCH 3/3] Simplify clamp_array --- resources/external_control.urscript | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/resources/external_control.urscript b/resources/external_control.urscript index b848f2ae..df0bbcdb 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -170,14 +170,12 @@ def clamp_array(values, clampValues): if length(values) != length(clampValues): popup("List of values has different length than list of clamp values.", error = True, blocking = True) end - pdiff = values - clampValues - mdiff = values + clampValues ret = values j = 0 while j < length(values): - if pdiff[j] > 0: + if values[j] > clampValues[j]: ret[j] = clampValues[j] - elif mdiff[j] < 0: + elif values[j] < -clampValues[j]: ret[j] = -clampValues[j] end j = j + 1