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..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/resources/external_control.urscript b/resources/external_control.urscript index 181e9e61..df0bbcdb 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,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): @@ -644,6 +673,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 +824,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 +870,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)