diff --git a/doc/architecture/script_command_interface.rst b/doc/architecture/script_command_interface.rst index 6a419d54..2059cec6 100644 --- a/doc/architecture/script_command_interface.rst +++ b/doc/architecture/script_command_interface.rst @@ -22,6 +22,8 @@ At the time of writing the ``ScriptCommandInterface`` provides the following fun for more information. - ``setFrictionCompensation()``: Set friction compensation for torque command. - ``ftRtdeInputEnable()``: Enable/disable FT RTDE input processing. +- ``setPDControllerGains()``: Set PD gains for the PD control loop running in the external control script. +- ``setMaxJointTorques()``: Set Max joint torques for the PD control loop running in the external control script. Communication protocol ---------------------- @@ -52,6 +54,8 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr - 6: endToolContact - 7: setFrictionCompensation - 8: ftRtdeInputEnable + - 9: setPDControllerGains + - 10: setMaxJointTorques 1-27 data fields specific to the command ===== ===== @@ -144,6 +148,24 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr 2 sensor_mass in kg (floating point) 3-5 sensor_mesurement_offset in m, displacement from the tool flange (3d floating point) 6-9 sensor_cog in m, displacement from the tool flange (3d floating point) + +.. table:: With setPDControllerGains command + :widths: auto + + ===== ===== + index meaning + ===== ===== + 1-6 Kp gains for each joint, used in the PD control loop running in the external control script (floating point) + 7-12 Kd gains for each joint, used in the PD control loop running in the external control script (floating point) + ===== ===== + +.. table:: With setMaxJointTorques command + :widths: auto + + ===== ===== + index meaning + ===== ===== + 1-6 max_joint_torques for each joint, used to clamp the torques between +-max_joint_torques before applying them to the robot in the PD control loop running in the external control script (floating point) ===== ===== .. note:: diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 2d52ae3d..8d34bc9b 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -63,3 +63,7 @@ target_link_libraries(instruction_executor ur_client_library::urcl) add_executable(external_fts_through_rtde external_fts_through_rtde.cpp) target_link_libraries(external_fts_through_rtde 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..6323091a --- /dev/null +++ b/examples/pd_controller_example.cpp @@ -0,0 +1,222 @@ +// 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 robot_version = g_my_robot->getUrDriver()->getVersion(); + if (robot_version < urcl::VersionInformation::fromString("5.23.0") || + (robot_version.major > 5 && robot_version < urcl::VersionInformation::fromString("10.10.0"))) + { + URCL_LOG_ERROR("This example requires a robot with at least version 5.23.0 / 10.10.0. Your robot has version %s.", + robot_version.toString().c_str()); + return 0; + } + } + + 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; + } + + return 0; +} diff --git a/examples/script_command_interface.cpp b/examples/script_command_interface.cpp index 6ada63f2..000a1c61 100644 --- a/examples/script_command_interface.cpp +++ b/examples/script_command_interface.cpp @@ -74,6 +74,13 @@ void sendScriptCommands() run_cmd("Disabling tool contact mode", []() { g_my_robot->getUrDriver()->endToolContact(); }); run_cmd("Setting friction_compensation variable to `true`", []() { g_my_robot->getUrDriver()->setFrictionCompensation(true); }); + run_cmd("Setting PD controller gains", []() { + g_my_robot->getUrDriver()->setPDControllerGains({ 500.0, 500.0, 300.0, 124.0, 124.0, 124.0 }, + { 44.72, 44.72, 34.64, 22.27, 22.27, 22.27 }); + }); + // The following will have no effect on PolyScope < 5.23 / 10.10 + run_cmd("Setting max joint torques", + []() { g_my_robot->getUrDriver()->setMaxJointTorques({ 27.0, 27.0, 14.0, 4.5, 4.5, 4.5 }); }); } URCL_LOG_INFO("Script command thread finished."); } diff --git a/include/ur_client_library/comm/control_mode.h b/include/ur_client_library/comm/control_mode.h index 51156da6..df173d4a 100644 --- a/include/ur_client_library/comm/control_mode.h +++ b/include/ur_client_library/comm/control_mode.h @@ -53,7 +53,8 @@ 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 + END ///< This is not an actual control mode, but used internally to get the number of control modes }; /*! @@ -64,8 +65,8 @@ 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 + ControlMode::MODE_SERVOJ, ControlMode::MODE_SPEEDJ, ControlMode::MODE_SPEEDL, + ControlMode::MODE_POSE, ControlMode::MODE_TORQUE, ControlMode::MODE_PD_CONTROLLER_JOINT }; // Control modes that doesn't require realtime communication diff --git a/include/ur_client_library/control/script_command_interface.h b/include/ur_client_library/control/script_command_interface.h index 5fead319..106b0aa4 100644 --- a/include/ur_client_library/control/script_command_interface.h +++ b/include/ur_client_library/control/script_command_interface.h @@ -158,11 +158,11 @@ class ScriptCommandInterface : public ReverseInterface bool endToolContact(); /*! - * \brief Set friction compensation for the torque_command. If true the torque command will compensate for friction, - * if false it will not. + * \brief Set friction compensation for the direct_torque command. If true the torque command will compensate for + * friction, if false it will not. * * \param friction_compensation_enabled Will set a friction_compensation_enabled variable in urscript, which will be - * used when calling torque_command + * used when calling direct_torque. * * \returns True, if the write was performed successfully, false otherwise. */ @@ -188,6 +188,32 @@ class ScriptCommandInterface : public ReverseInterface const vector3d_t& sensor_measuring_offset = { 0.0, 0.0, 0.0 }, const vector3d_t& sensor_cog = { 0.0, 0.0, 0.0 }); + /*! + * \brief Set gains for the PD controller running in the external control script. The PD controller computes joint + * torques based on either tcp poses or joint poses and applies the torques to the robot using the direct_torque + * function. The gains can be used to change the response of the controller. Be aware that changing the controller + * response can make it unstable. + * The PD controller can be used without explicitly defining those gains, as it contains a set of default gains for + * each robot model. + * + * \param kp A vector6d of proportional gains for each of the joints in the robot. + * \param kd A vector6d of derivative gains for each of the joints in the robot. + * + * \returns True, if the write was performed successfully, false otherwise. + */ + bool setPDControllerGains(const urcl::vector6d_t* kp, const urcl::vector6d_t* kd); + + /*! + * \brief Set the maximum joint torques for the PD controller running in the external control script. The PD + * controller will clamp the torques between +-max_joint_torques before aplying them to the robot using the + * direct_torque function. + * + * \param max_joint_torques A vector6d of the maximum joint torques for each of the joints. + * + * \returns True, if the write was performed successfully, false otherwise. + */ + bool setMaxJointTorques(const urcl::vector6d_t* max_joint_torques); + /*! * \brief Returns whether a client/robot is connected to this server. * @@ -227,6 +253,8 @@ class ScriptCommandInterface : public ReverseInterface END_TOOL_CONTACT = 6, ///< End detecting tool contact SET_FRICTION_COMPENSATION = 7, ///< Set friction compensation FT_RTDE_INPUT_ENABLE = 8, ///< Enable FT RTDE input + SET_PD_CONTROLLER_GAINS = 9, ///< Set PD controller gains + SET_MAX_JOINT_TORQUES = 10, ///< Set max joint torques }; /*! 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..5a4a7be9 --- /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 " << robotTypeString(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 " << robotTypeString(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/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 15a4fd25..1f4fb3e6 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -716,11 +716,11 @@ class UrDriver bool endToolContact(); /*! - * \brief Set friction compensation for the torque_command. If true the torque command will compensate for friction, - * if false it will not. + * \brief Set friction compensation for the direct_torque command. If true the torque command will compensate for + * friction, if false it will not. * * \param friction_compensation_enabled Will set a friction_compensation_enabled variable in urscript, which will be - * used when calling torque_command + * used when calling direct_torque. * * \returns True, if the write was performed successfully, false otherwise. */ @@ -746,6 +746,32 @@ class UrDriver const vector3d_t& sensor_measuring_offset = { 0.0, 0.0, 0.0 }, const vector3d_t& sensor_cog = { 0.0, 0.0, 0.0 }); + /*! + * \brief Set gains for the PD controller running in the external control script. The PD controller computes joint + * torques based on either tcp poses or joint poses and applies the torques to the robot using the direct_torque + * function. The gains can be used to change the response of the controller. Be aware that changing the controller + * response can make it unstable. + * The PD controller can be used without explicitly defining those gains, as it contains a set of default gains for + * each robot model. + * + * \param kp A vector6d of proportional gains for each of the joints in the robot. + * \param kd A vector6d of derivative gains for each of the joints in the robot. + * + * \returns True, if the write was performed successfully, false otherwise. + */ + bool setPDControllerGains(const urcl::vector6d_t& kp, const urcl::vector6d_t& kd); + + /*! + * \brief Set the maximum joint torques for the PD controller running in the external control script. The PD + * controller will clamp the torques between +-max_joint_torques before aplying them to the robot using the + * direct_torque function. + * + * \param max_joint_torques A vector6d of the maximum joint torques for each of the joints. + * + * \returns True, if the write was performed successfully, false otherwise. + */ + bool setMaxJointTorques(const urcl::vector6d_t& max_joint_torques); + /*! * \brief Write a keepalive signal only. * diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 301781a4..8046745c 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -28,6 +28,7 @@ MODE_POSE = 5 MODE_FREEDRIVE = 6 MODE_TOOL_IN_CONTACT = 7 MODE_TORQUE = 8 +MODE_PD_CONTROLLER_JOINT = 9 # Data dimensions of the message received on the reverse interface REVERSE_INTERFACE_DATA_DIMENSION = 8 @@ -56,6 +57,8 @@ START_TOOL_CONTACT = 5 END_TOOL_CONTACT = 6 SET_FRICTION_COMPENSATION = 7 FT_RTDE_INPUT_ENABLE = 8 +SET_PD_CONTROLLER_GAINS = 9 +SET_MAX_JOINT_TORQUES = 10 SCRIPT_COMMAND_DATA_DIMENSION = 28 FREEDRIVE_MODE_START = 1 @@ -90,6 +93,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 @@ -159,6 +164,31 @@ 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 + ret = values + j = 0 + while j < length(values): + if values[j] > clampValues[j]: + ret[j] = clampValues[j] + elif values[j] < -clampValues[j]: + 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): @@ -716,6 +746,17 @@ thread servoThreadP(): stopj(STOPJ_ACCELERATION) end +thread PDControlThread(): + while control_mode == MODE_PD_CONTROLLER_JOINT: + 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) + direct_torque(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()) @@ -818,6 +859,11 @@ thread script_commands(): # This has a known error that the resulting torques are computed with opposite sign. enable_external_ft_sensor(enabled, sensor_mass, sensor_offset, sensor_cog) {% endif %} + elif command == SET_PD_CONTROLLER_GAINS: + pd_controller_gains.kp = [raw_command[2] / MULT_jointstate, raw_command[3] / MULT_jointstate, raw_command[4] / MULT_jointstate, raw_command[5] / MULT_jointstate, raw_command[6] / MULT_jointstate, raw_command[7] / MULT_jointstate] + pd_controller_gains.kd = [raw_command[8] / MULT_jointstate, raw_command[9] / MULT_jointstate, raw_command[10] / MULT_jointstate, raw_command[11] / MULT_jointstate, raw_command[12] / MULT_jointstate, raw_command[13] / MULT_jointstate] + elif command == SET_MAX_JOINT_TORQUES: + max_joint_torques = [raw_command[2] / MULT_jointstate, raw_command[3] / MULT_jointstate, raw_command[4] / MULT_jointstate, raw_command[5] / MULT_jointstate, raw_command[6] / MULT_jointstate, raw_command[7] / MULT_jointstate] end end end @@ -885,6 +931,9 @@ 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() end end @@ -925,6 +974,9 @@ 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) 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/control/script_command_interface.cpp b/src/control/script_command_interface.cpp index 01342c29..048e1f40 100644 --- a/src/control/script_command_interface.cpp +++ b/src/control/script_command_interface.cpp @@ -293,6 +293,68 @@ bool ScriptCommandInterface::ftRtdeInputEnable(const bool enabled, const double return server_.write(client_fd_, buffer, sizeof(buffer), written); } +bool ScriptCommandInterface::setPDControllerGains(const urcl::vector6d_t* kp, const urcl::vector6d_t* kd) +{ + robotVersionSupportsCommandOrWarn(urcl::VersionInformation::fromString("5.23.0"), + urcl::VersionInformation::fromString("10.10.0"), __func__); + const int message_length = 13; + uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH]; + uint8_t* b_pos = buffer; + + int32_t val = htobe32(toUnderlying(ScriptCommand::SET_PD_CONTROLLER_GAINS)); + b_pos += append(b_pos, val); + + for (auto const& p_gain : *kp) + { + val = htobe32(static_cast(round(p_gain * MULT_JOINTSTATE))); + b_pos += append(b_pos, val); + } + + for (auto const& d_gain : *kd) + { + val = htobe32(static_cast(round(d_gain * MULT_JOINTSTATE))); + b_pos += append(b_pos, val); + } + + // writing zeros to allow usage with other script commands + for (size_t i = message_length; i < MAX_MESSAGE_LENGTH; i++) + { + val = htobe32(0); + b_pos += append(b_pos, val); + } + size_t written; + + return server_.write(client_fd_, buffer, sizeof(buffer), written); +} + +bool ScriptCommandInterface::setMaxJointTorques(const urcl::vector6d_t* max_joint_torques) +{ + robotVersionSupportsCommandOrWarn(urcl::VersionInformation::fromString("5.23.0"), + urcl::VersionInformation::fromString("10.10.0"), __func__); + const int message_length = 7; + uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH]; + uint8_t* b_pos = buffer; + + int32_t val = htobe32(toUnderlying(ScriptCommand::SET_MAX_JOINT_TORQUES)); + b_pos += append(b_pos, val); + + for (auto const& max_torque : *max_joint_torques) + { + val = htobe32(static_cast(round(max_torque * MULT_JOINTSTATE))); + b_pos += append(b_pos, val); + } + + // writing zeros to allow usage with other script commands + for (size_t i = message_length; i < MAX_MESSAGE_LENGTH; i++) + { + val = htobe32(0); + b_pos += append(b_pos, val); + } + size_t written; + + return server_.write(client_fd_, buffer, sizeof(buffer), written); +} + bool ScriptCommandInterface::clientConnected() { return client_connected_; @@ -360,4 +422,4 @@ bool ScriptCommandInterface::robotVersionSupportsCommandOrWarn(const VersionInfo } } // namespace control -} // namespace urcl \ No newline at end of file +} // namespace urcl diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index e860722e..cebad98d 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -36,6 +36,7 @@ #include "ur_client_library/exceptions.h" #include "ur_client_library/helpers.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 @@ -52,6 +53,8 @@ static const std::string SERVER_IP_REPLACE("SERVER_IP_REPLACE"); static const std::string SERVER_PORT_REPLACE("SERVER_PORT_REPLACE"); static const std::string TRAJECTORY_PORT_REPLACE("TRAJECTORY_SERVER_PORT_REPLACE"); static const std::string SCRIPT_COMMAND_PORT_REPLACE("SCRIPT_COMMAND_SERVER_PORT_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"); UrDriver::~UrDriver() { @@ -140,6 +143,26 @@ void UrDriver::init(const UrDriverConfiguration& config) } data[BEGIN_REPLACE] = begin_replace.str(); + data["ROBOT_SOFTWARE_VERSION"] = getVersion(); + const RobotType robot_type = primary_client_->getRobotType(); + + const control::PDControllerGains pd_gains = control::getPdGainsFromRobotType(robot_type); + std::stringstream pd_gains_ss; + if (robot_version_ < urcl::VersionInformation::fromString("5.10.0")) + { + // Structs are only available in URScript 5.10 and later. It isn't used pre 5.23, so we can safely set it to 0. + pd_gains_ss << 0; + } + else + { + pd_gains_ss << "struct(kp=" << pd_gains.kp << ", kd=" << pd_gains.kd << ")"; + } + data[PD_CONTROLLER_GAINS_REPLACE] = pd_gains_ss.str(); + + std::stringstream max_torques_ss; + max_torques_ss << control::getMaxTorquesFromRobotType(robot_type); + data[MAX_JOINT_TORQUE_REPLACE] = max_torques_ss.str(); + data["ROBOT_SOFTWARE_VERSION"] = getVersion(); script_reader_.reset(new control::ScriptReader()); @@ -553,6 +576,54 @@ bool UrDriver::ftRtdeInputEnable(const bool enabled, const double sensor_mass, } } +bool UrDriver::setPDControllerGains(const urcl::vector6d_t& kp, const urcl::vector6d_t& kd) +{ + if (!std::all_of(kp.begin(), kp.end(), [](double v) { return v >= 0.0; })) + { + throw InvalidRange("kp should be larger than zero"); + } + + if (!std::all_of(kd.begin(), kd.end(), [](double v) { return v >= 0.0; })) + { + throw InvalidRange("kd should be larger than zero"); + } + + if (script_command_interface_->clientConnected()) + { + return script_command_interface_->setPDControllerGains(&kp, &kd); + } + else + { + URCL_LOG_ERROR("Script command interface is not running. Unable to set PD Controller gains."); + return 0; + } +} + +bool UrDriver::setMaxJointTorques(const urcl::vector6d_t& max_joint_torques) +{ + const urcl::vector6d_t max_torques_for_robot_type = + control::getMaxTorquesFromRobotType(primary_client_->getRobotType()); + if (!std::equal(max_joint_torques.begin(), max_joint_torques.end(), max_torques_for_robot_type.begin(), + [](double v1, double v2) { return v1 <= v2 && v1 >= 0.0; })) + { + std::stringstream ss; + ss << "The max joint torques should be smaller or equal to the maximum joint torques for the robot type and larger " + "than zero. Provided max joint torques " + << max_joint_torques << " and maximum joint torques for the robot type " << max_torques_for_robot_type; + throw InvalidRange(ss.str().c_str()); + } + + if (script_command_interface_->clientConnected()) + { + return script_command_interface_->setMaxJointTorques(&max_joint_torques); + } + else + { + URCL_LOG_ERROR("Script command interface is not running. Unable to set max joint torques."); + return 0; + } +} + bool UrDriver::writeKeepalive(const RobotReceiveTimeout& robot_receive_timeout) { vector6d_t* fake = nullptr; diff --git a/tests/test_reverse_interface.cpp b/tests/test_reverse_interface.cpp index a365b6dd..9898adf5 100644 --- a/tests/test_reverse_interface.cpp +++ b/tests/test_reverse_interface.cpp @@ -436,6 +436,12 @@ TEST_F(ReverseIntefaceTest, write_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); } TEST_F(ReverseIntefaceTest, write_freedrive_control_message) diff --git a/tests/test_script_command_interface.cpp b/tests/test_script_command_interface.cpp index 211d4ce5..7c60666b 100644 --- a/tests/test_script_command_interface.cpp +++ b/tests/test_script_command_interface.cpp @@ -431,7 +431,6 @@ TEST_F(ScriptCommandInterfaceTest, test_ft_rtde_input_enable) { // Wait for the client to connect to the server waitForClientConnection(); - double sensor_mass = 1.42; vector3d_t sensor_measuring_offset = { 0.1, 0.2, 0.3 }; vector3d_t sensor_cog = { 0.01, 0.02, 0.03 }; @@ -482,6 +481,76 @@ TEST_F(ScriptCommandInterfaceTest, test_ft_rtde_input_enable) EXPECT_EQ(received_enabled, false); } +TEST_F(ScriptCommandInterfaceTest, test_set_pd_controller_gains) +{ + // Wait for the client to connect to the server + waitForClientConnection(); + + urcl::vector6d_t kp = { 220.2, 220.2, 300.0, 10.32, 10.32, 10.32 }; + urcl::vector6d_t kd = { 29.68, 29.68, 35.0, 6.4, 6.4, 6.4 }; + script_command_interface_->setPDControllerGains(&kp, &kd); + + int32_t command; + std::vector message; + client_->readMessage(command, message); + + // 8 is set PD controller gains + int32_t expected_command = 9; + EXPECT_EQ(command, expected_command); + + int32_t message_idx = 0; + + for (auto& p_gain : kp) + { + const double received_gain = (double)message[message_idx] / script_command_interface_->MULT_JOINTSTATE; + EXPECT_EQ(p_gain, received_gain); + message_idx = message_idx + 1; + } + + for (auto& d_gain : kd) + { + const double received_gain = (double)message[message_idx] / script_command_interface_->MULT_JOINTSTATE; + EXPECT_EQ(d_gain, received_gain); + message_idx = message_idx + 1; + } + + // The rest of the message should be zero + int32_t message_sum = std::accumulate(std::begin(message) + message_idx, std::end(message), 0); + int32_t expected_message_sum = 0; + EXPECT_EQ(message_sum, expected_message_sum); +} + +TEST_F(ScriptCommandInterfaceTest, test_set_max_joint_torques) +{ + // Wait for the client to connect to the server + waitForClientConnection(); + + urcl::vector6d_t max_joint_torques = { 100.0, 150.0, 21.2, 10.32, 10.32, 10.32 }; + script_command_interface_->setMaxJointTorques(&max_joint_torques); + + int32_t command; + std::vector message; + client_->readMessage(command, message); + + // 9 is set max joint torques + int32_t expected_command = 10; + EXPECT_EQ(command, expected_command); + + int32_t message_idx = 0; + + for (auto& max_torque : max_joint_torques) + { + const double received_max_torque = (double)message[message_idx] / script_command_interface_->MULT_JOINTSTATE; + EXPECT_EQ(max_torque, received_max_torque); + message_idx = message_idx + 1; + } + + // The rest of the message should be zero + int32_t message_sum = std::accumulate(std::begin(message) + message_idx, std::end(message), 0); + int32_t expected_message_sum = 0; + EXPECT_EQ(message_sum, expected_message_sum); +} + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv); diff --git a/tests/test_script_reader.cpp b/tests/test_script_reader.cpp index 21d949e6..9b8688ee 100644 --- a/tests/test_script_reader.cpp +++ b/tests/test_script_reader.cpp @@ -464,6 +464,8 @@ TEST_F(ScriptReaderTest, TestParsingExternalControl) data["SERVER_PORT_REPLACE"] = "50001"; data["TRAJECTORY_SERVER_PORT_REPLACE"] = "50003"; data["SCRIPT_COMMAND_SERVER_PORT_REPLACE"] = "50004"; + data["PD_CONTROLLER_GAINS_REPLACE"] = "struct(kp=100, kd=20)"; + data["MAX_JOINT_TORQUE_REPLACE"] = "[150, 150, 150, 28, 28, 28]"; data["ROBOT_SOFTWARE_VERSION"] = urcl::VersionInformation::fromString("3.12.1"); std::string processed_script = reader.readScriptFile(existing_script_file, data);