From d0b39294a98a77dd4425a550c42068bbf2428937 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 7 Jul 2025 21:59:42 +0200 Subject: [PATCH 01/12] Support optimove motions in InstructionExecutor --- CMakeLists.txt | 1 + doc/architecture/instruction_executor.rst | 2 + doc/examples/instruction_executor.rst | 9 ++- examples/instruction_executor.cpp | 6 +- .../control/motion_primitives.h | 38 ++++++++++ .../ur/instruction_executor.h | 37 ++++++++++ resources/external_control.urscript | 61 +++++++++++++++ src/control/trajectory_point_interface.cpp | 25 +++++++ src/ur/instruction_executor.cpp | 13 ++++ src/ur/ur_driver.cpp | 3 + tests/test_instruction_executor.cpp | 74 +++++++++++++++++++ 11 files changed, 264 insertions(+), 5 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a6aeebdfb..d26182de8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -17,6 +17,7 @@ endif() add_library(urcl src/comm/tcp_socket.cpp src/comm/tcp_server.cpp + src/control/motion_primitives.cpp src/control/reverse_interface.cpp src/control/script_reader.cpp src/control/script_sender.cpp diff --git a/doc/architecture/instruction_executor.rst b/doc/architecture/instruction_executor.rst index 297d8e666..c143d983f 100644 --- a/doc/architecture/instruction_executor.rst +++ b/doc/architecture/instruction_executor.rst @@ -12,6 +12,8 @@ to point motions easily accessible. Currently, it supports the following instruc * Execute MoveL point to point motions * Execute MoveP point to point motions * Execute MoveC circular motions +* Execute OptimoveJ point to point motions (For PolyScope 5.21 / PolyScope 10.8 and later) +* Execute OptimoveL point to point motions (For PolyScope 5.21 / PolyScope 10.8 and later) * Execute sequences consisting of the motion primitives above The Instruction Executor uses the :ref:`trajectory_point_interface` and the diff --git a/doc/examples/instruction_executor.rst b/doc/examples/instruction_executor.rst index 697a26bfc..a2f67a1cc 100644 --- a/doc/examples/instruction_executor.rst +++ b/doc/examples/instruction_executor.rst @@ -44,10 +44,11 @@ To run a sequence of motions, create an :end-at: instruction_executor->executeMotion(motion_sequence); Each element in the motion sequence can be a different motion type. In the example, there are two -``MoveJ`` motions, a ``MoveL`` motion and a ``MoveP`` motion. The primitives' parameters are directly forwarded to -the underlying script functions, so the parameter descriptions for them apply, as well. -Particularly, you may want to choose between either a time-based execution speed or an acceleration -/ velocity parametrization for some move functions. The latter will be ignored if a time > 0 is given. +``MoveJ`` motions, a ``MoveL`` motion, a ``MoveP`` motion, a ``OptimiveJ`` motion and a +``OptimoveL`` motion. The primitives' parameters are directly forwarded to the underlying script +functions, so the parameter descriptions for them apply, as well. Particularly, you may want to +choose between either a time-based execution speed or an acceleration / velocity parametrization +for some move functions. The latter will be ignored if a time > 0 is given. Please refer to the script manual for details. diff --git a/examples/instruction_executor.cpp b/examples/instruction_executor.cpp index f629b3cc6..229b9f02a 100644 --- a/examples/instruction_executor.cpp +++ b/examples/instruction_executor.cpp @@ -58,7 +58,7 @@ int main(int argc, char* argv[]) robot_ip = std::string(argv[1]); } - bool headless_mode = true; + bool headless_mode = false; g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode, "external_control.urp"); if (!g_my_robot->getUrDriver()->checkCalibration(CALIBRATION_CHECKSUM)) @@ -91,6 +91,10 @@ int main(int argc, char* argv[]) std::chrono::seconds(2)), std::make_shared(urcl::Pose{ -0.203, 0.463, 0.559, 0.68, -1.083, -2.076 }, 0.1, 0.4, 0.4), + std::make_shared(urcl::vector6d_t{ -1.57, -1.57, 1.6, -0.5, 0.4, 0.3 }, 0.1, 0.4, + 0.7), + std::make_shared(urcl::Pose(-0.203, 0.263, 0.559, 0.68, -1.083, -2.076), 0.1, + 0.4, 0.7), }; instruction_executor->executeMotion(motion_sequence); diff --git a/include/ur_client_library/control/motion_primitives.h b/include/ur_client_library/control/motion_primitives.h index 10337cf07..ce21c200a 100644 --- a/include/ur_client_library/control/motion_primitives.h +++ b/include/ur_client_library/control/motion_primitives.h @@ -46,6 +46,8 @@ enum class MotionType : uint8_t MOVEL = 1, MOVEP = 2, MOVEC = 3, + OPTIMOVEJ = 4, + OPTIMOVEL = 5, SPLINE = 51, UNKNOWN = 255 }; @@ -66,6 +68,8 @@ struct MotionPrimitive double acceleration; double velocity; double blend_radius; + + virtual bool validate() const; }; struct MoveJPrimitive : public MotionPrimitive @@ -162,6 +166,40 @@ struct SplinePrimitive : public MotionPrimitive vector6d_t target_velocities; std::optional target_accelerations; }; + +struct OptimoveJPrimitive : public MotionPrimitive +{ + OptimoveJPrimitive(const urcl::vector6d_t& target, const double blend_radius = 0, + const double acceleration_fraction = 0.3, const double velocity_fraction = 0.3) + { + type = MotionType::OPTIMOVEJ; + target_joint_configuration = target; + this->blend_radius = blend_radius; + this->acceleration = acceleration_fraction; + this->velocity = velocity_fraction; + } + + urcl::vector6d_t target_joint_configuration; + + bool validate() const override; +}; + +struct OptimoveLPrimitive : public MotionPrimitive +{ + OptimoveLPrimitive(const urcl::Pose& target, const double blend_radius = 0, const double acceleration_fraction = 0.3, + const double velocity_fraction = 0.3) + { + type = MotionType::OPTIMOVEL; + target_pose = target; + this->blend_radius = blend_radius; + this->acceleration = acceleration_fraction; + this->velocity = velocity_fraction; + } + + bool validate() const override; + + urcl::Pose target_pose; +}; } // namespace control } // namespace urcl #endif // UR_CLIENT_LIBRARY_MOTION_PRIMITIVES_H_INCLUDED diff --git a/include/ur_client_library/ur/instruction_executor.h b/include/ur_client_library/ur/instruction_executor.h index cb158b235..664b4e1c3 100644 --- a/include/ur_client_library/ur/instruction_executor.h +++ b/include/ur_client_library/ur/instruction_executor.h @@ -132,6 +132,42 @@ class InstructionExecutor bool moveC(const urcl::Pose& via, const urcl::Pose& target, const double acceleration = 1.4, const double velocity = 1.04, const double blend_radius = 0.0, const int32_t mode = 0); + /** + * \brief Move the robot to a joint target using optimoveJ. + * + * This function will move the robot to the given joint target using the optimoveJ motion + * primitive. The robot will move with the given acceleration and velocity fractions. + * + * \param target The joint target to move to. + * \param acceleration_fraction The fraction of the maximum acceleration to use for the motion + * (0.0 < fraction <= 1.0). + * \param velocity_fraction The fraction of the maximum velocity to use for the motion_sequence + * (0.0 < fraction <= 1.0). + * \param blend_radius The blend radius to use for the motion. + * + * \return True if the robot has reached the target, false otherwise. + */ + bool optimoveJ(const urcl::vector6d_t& target, const double acceleration_fraction = 0.3, + const double velocity_fraction = 0.3, const double blend_radius = 0); + + /** + * \brief Move the robot to a pose target using optimoveL. + * + * This function will move the robot to the given pose target using the optimoveL motion + * primitive. The robot will move with the given acceleration and velocity fractions. + * + * \param target The pose target to move to. + * \param acceleration_fraction The fraction of the maximum acceleration to use for the motion + * (0.0 < fraction <= 1.0). + * \param velocity_fraction The fraction of the maximum velocity to use for the motion_sequence + * (0.0 < fraction <= 1.0). + * \param blend_radius The blend radius to use for the motion. + * + * \return True if the robot has reached the target, false otherwise. + */ + bool optimoveL(const urcl::Pose& target, const double acceleration_fraction = 0.3, + const double velocity_fraction = 0.3, const double blend_radius = 0); + /** * \brief Cancel the current motion. * @@ -154,6 +190,7 @@ class InstructionExecutor private: void trajDoneCallback(const urcl::control::TrajectoryResult& result); void trajDisconnectCallback(const int filedescriptor); + std::shared_ptr driver_; std::atomic trajectory_running_ = false; std::atomic cancel_requested_ = false; diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 61a56bf85..02a902f8b 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -8,6 +8,8 @@ textmsg("ExternalControl: steptime=", steptime) MULT_jointstate = {{JOINT_STATE_REPLACE}} MULT_time = {{TIME_REPLACE}} +DEBUG = False + STOPJ_ACCELERATION = 4.0 #Constants @@ -35,6 +37,8 @@ MOTION_TYPE_MOVEJ = 0 MOTION_TYPE_MOVEL = 1 MOTION_TYPE_MOVEP = 2 MOTION_TYPE_MOVEC = 3 +MOTION_TYPE_OPTIMOVEJ = 4 +MOTION_TYPE_OPTIMOVEL = 5 MOTION_TYPE_SPLINE = 51 TRAJECTORY_DATA_DIMENSION = 3 * 6 + 1 @@ -551,6 +555,63 @@ thread trajectoryThread(): clear_remaining_trajectory_points() trajectory_result = TRAJECTORY_RESULT_FAILURE end + + # OptimoveJ point + elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEJ: + acceleration = raw_point[13] / MULT_jointstate + velocity = raw_point[7] / MULT_jointstate + {% if ROBOT_SOFTWARE_VERSION >= v5.21.0 %} + {% if ROBOT_SOFTWARE_VERSION < v6.0.0 %} + optimovej(q, a = acceleration, v = velocity, r = blend_radius) + {% elif ROBOT_SOFTWARE_VERSION >= v10.8.0 %} + optimovej(q, a = acceleration, v = velocity, r = blend_radius) + {% else %} + popup("Optimovej is only supported from software 10.8.0 and upwards.", error=True, blocking=True) + {% endif %} + {% else %} + popup("Optimovej is only supported from software 5.21.0 and upwards.", error=True, blocking=True) + {% endif %} + + if DEBUG: + textmsg(str_cat("optimovej(", str_cat( + str_cat("q=", q), str_cat( + str_cat(", a=", acceleration), str_cat( + str_cat(", v=", velocity), str_cat( + str_cat(", r=", blend_radius), ")")))))) + end + + # reset old acceleration + spline_qdd = [0, 0, 0, 0, 0, 0] + spline_qd = [0, 0, 0, 0, 0, 0] + + # OptimoveL point + elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEL: + acceleration = raw_point[13] / MULT_jointstate + velocity = raw_point[7] / MULT_jointstate + + {% if ROBOT_SOFTWARE_VERSION >= v5.21.0 %} + {% if ROBOT_SOFTWARE_VERSION < v6.0.0 %} + optimovel(p[q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, r = blend_radius) + {% elif ROBOT_SOFTWARE_VERSION >= v10.8.0 %} + optimovel(p[q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, r = blend_radius) + {% else %} + popup("Optimovel is only supported from software 10.8.0 and upwards.", error=True, blocking=True) + {% endif %} + {% else %} + popup("Optimovel is only supported from software 5.21.0 and upwards.", error=True, blocking=True) + {% endif %} + + if DEBUG: + textmsg(str_cat("optimovel(", str_cat( + str_cat("q=", p[q[0], q[1], q[2], q[3], q[4], q[5]]), str_cat( + str_cat(", a=", acceleration), str_cat( + str_cat(", v=", velocity), str_cat( + str_cat(", r=", blend_radius), ")")))))) + end + + # reset old acceleration + spline_qdd = [0, 0, 0, 0, 0, 0] + spline_qd = [0, 0, 0, 0, 0, 0] end else: textmsg("Receiving trajectory point failed!") diff --git a/src/control/trajectory_point_interface.cpp b/src/control/trajectory_point_interface.cpp index 85c6221bc..cf61506fb 100644 --- a/src/control/trajectory_point_interface.cpp +++ b/src/control/trajectory_point_interface.cpp @@ -62,6 +62,12 @@ TrajectoryPointInterface::TrajectoryPointInterface(uint32_t port) : ReverseInter bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptr primitive) { + if (!primitive->validate()) + { + URCL_LOG_ERROR("Motion primitive validation failed."); + return false; + } + if (client_fd_ == -1) { return false; @@ -131,6 +137,25 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptr(primitive); + first_block = optimovej_primitive->target_joint_configuration; + second_block.fill(primitive->velocity); + third_block.fill(primitive->acceleration); + break; + } + case control::MotionType::OPTIMOVEL: + { + URCL_LOG_INFO("Executing OptimoveL motion."); + auto optimovel_primitive = std::static_pointer_cast(primitive); + first_block = { optimovel_primitive->target_pose.x, optimovel_primitive->target_pose.y, + optimovel_primitive->target_pose.z, optimovel_primitive->target_pose.rx, + optimovel_primitive->target_pose.ry, optimovel_primitive->target_pose.rz }; + second_block.fill(primitive->velocity); + third_block.fill(primitive->acceleration); + break; + } default: throw UnsupportedMotionType(); } diff --git a/src/ur/instruction_executor.cpp b/src/ur/instruction_executor.cpp index 84c66d6e9..5828c830e 100644 --- a/src/ur/instruction_executor.cpp +++ b/src/ur/instruction_executor.cpp @@ -99,6 +99,7 @@ bool urcl::InstructionExecutor::moveJ(const urcl::vector6d_t& target, const doub return executeMotion({ std::make_shared( target, blend_radius, std::chrono::milliseconds(static_cast(time * 1000)), acceleration, velocity) }); } + bool urcl::InstructionExecutor::moveL(const urcl::Pose& target, const double acceleration, const double velocity, const double time, const double blend_radius) { @@ -119,6 +120,18 @@ bool urcl::InstructionExecutor::moveC(const urcl::Pose& via, const urcl::Pose& t { std::make_shared(via, target, blend_radius, acceleration, velocity, mode) }); } +bool urcl::InstructionExecutor::optimoveJ(const urcl::vector6d_t& target, const double acceleration, + const double velocity, const double blend_radius) +{ + return executeMotion({ std::make_shared(target, blend_radius, acceleration, velocity) }); +} + +bool urcl::InstructionExecutor::optimoveL(const urcl::Pose& target, const double acceleration, const double velocity, + const double blend_radius) +{ + return executeMotion({ std::make_shared(target, blend_radius, acceleration, velocity) }); +} + bool urcl::InstructionExecutor::cancelMotion() { cancel_requested_ = true; diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index ad4af76eb..08bc49a2f 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -127,6 +127,9 @@ void UrDriver::init(const UrDriverConfiguration& config) << config.tool_comm_setup->getTxIdleChars() << ")"; } data[BEGIN_REPLACE] = begin_replace.str(); + + data["ROBOT_SOFTWARE_VERSION"] = getVersion(); + script_reader_.reset(new control::ScriptReader()); std::string prog = script_reader_->readScriptFile(config.script_file, data); diff --git a/tests/test_instruction_executor.cpp b/tests/test_instruction_executor.cpp index 4cab876f1..66cb07986 100644 --- a/tests/test_instruction_executor.cpp +++ b/tests/test_instruction_executor.cpp @@ -106,6 +106,10 @@ TEST_F(InstructionExecutorTest, execute_motion_sequence_success) std::chrono::seconds(2)), std::make_shared(urcl::Pose{ -0.203, 0.463, 0.559, 0.68, -1.083, -2.076 }, 0.1, std::chrono::seconds(2)), + std::make_shared(urcl::vector6d_t{ -1.57, -1.57, 1.6, -0.5, 0.4, 0.3 }, 0.1, 0.4, + 0.7), + std::make_shared(urcl::Pose(-0.203, 0.263, 0.559, 0.68, -1.083, -2.076), 0.1, + 0.4, 0.7), }; ASSERT_TRUE(executor_->executeMotion(motion_sequence)); } @@ -381,6 +385,76 @@ TEST_F(InstructionExecutorTest, movec_succeeds) { -0.209, 0.425, 0.841, 1.16, -0.477, -2.553 }, 0.1, 0.25, 0.025, 0)); } +TEST_F(InstructionExecutorTest, optimovej_succeeds) +{ + if (robotVersionLessThan(g_ROBOT_IP, "5.21.0")) + { + GTEST_SKIP_("optimoveJ is not supported on robots with a version lower than 5.21.0."); + } + else if (!robotVersionLessThan(g_ROBOT_IP, "10.0.0") && robotVersionLessThan(g_ROBOT_IP, "10.8.0")) + { + GTEST_SKIP_("optimoveJ is not supported on PolyScope X with a version lower than 10.8.0."); + } + // move to a feasible starting pose + ASSERT_TRUE(executor_->moveJ({ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 })); + + ASSERT_TRUE(executor_->optimoveJ({ -1.57, -1.57, 1.6, -0.5, 0.4, 0.3 }, 0.4, 0.7, 0.1)); + ASSERT_TRUE(executor_->optimoveJ({ -1.57, -1.57, 1.6, -0.5, 0.4, 0.1 }, 1.0, 1.0, 0.1)); +} + +TEST_F(InstructionExecutorTest, optimovel_succeeds) +{ + if (robotVersionLessThan(g_ROBOT_IP, "5.21.0")) + { + GTEST_SKIP_("optimoveL is not supported on robots with a version lower than 5.21.0."); + } + else if (!robotVersionLessThan(g_ROBOT_IP, "10.0.0") && robotVersionLessThan(g_ROBOT_IP, "10.8.0")) + { + GTEST_SKIP_("optimoveL is not supported on PolyScope X with a version lower than 10.8.0."); + } + // move to a feasible starting pose + ASSERT_TRUE(executor_->moveJ({ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 })); + + ASSERT_TRUE(executor_->optimoveL({ -0.203, 0.263, 0.559, 0.68, -1.083, -2.076 }, 0.4, 0.7, 0.1)); + ASSERT_TRUE(executor_->optimoveL({ -0.203, 0.263, 0.459, 0.68, -1.083, -2.076 }, 1.0, 1.0, 0.1)); +} + +TEST_F(InstructionExecutorTest, optimovej_with_illegal_parameters_fails) +{ + // Negative acceleration + ASSERT_FALSE(executor_->optimoveJ({ -1.57, -1.57, 1.6, -0.5, 0.4, 0.3 }, -0.4, 0.7)); + // Acceleration of 0 + ASSERT_FALSE(executor_->optimoveJ({ -1.57, -1.57, 1.6, -0.5, 0.4, 0.3 }, 0.0, 0.7)); + // Acceleration > 1 + ASSERT_FALSE(executor_->optimoveJ({ -1.57, -1.57, 1.6, -0.5, 0.4, 0.3 }, 1.2, 0.7)); + // negative velocity + ASSERT_FALSE(executor_->optimoveJ({ -1.57, -1.57, 1.6, -0.5, 0.4, 0.3 }, 0.4, -0.7)); + // Velocity of 0 + ASSERT_FALSE(executor_->optimoveJ({ -1.57, -1.57, 1.6, -0.5, 0.4, 0.3 }, 0.4, 0.0)); + // Velocity > 1 + ASSERT_FALSE(executor_->optimoveJ({ -1.57, -1.57, 1.6, -0.5, 0.4, 0.3 }, 0.4, 1.2)); + // Negative blend radius + ASSERT_FALSE(executor_->optimoveJ({ -1.57, -1.57, 1.6, -0.5, 0.4, 0.3 }, 0.4, 0.7, -0.1)); +} + +TEST_F(InstructionExecutorTest, optimovel_with_illegal_parameters_fails) +{ + // Negative acceleration + ASSERT_FALSE(executor_->optimoveL({ -0.203, 0.263, 0.559, 0.68, -1.083, -2.076 }, -0.4, 0.7)); + // Acceleration of 0 + ASSERT_FALSE(executor_->optimoveL({ -0.203, 0.263, 0.559, 0.68, -1.083, -2.076 }, 0.0, 0.7)); + // Acceleration > 1 + ASSERT_FALSE(executor_->optimoveL({ -0.203, 0.263, 0.559, 0.68, -1.083, -2.076 }, 1.2, 0.7)); + // negative velocity + ASSERT_FALSE(executor_->optimoveL({ -0.203, 0.263, 0.559, 0.68, -1.083, -2.076 }, 0.4, -0.7)); + // Velocity of 0 + ASSERT_FALSE(executor_->optimoveL({ -0.203, 0.263, 0.559, 0.68, -1.083, -2.076 }, 0.4, 0.0)); + // Velocity > 1 + ASSERT_FALSE(executor_->optimoveL({ -0.203, 0.263, 0.559, 0.68, -1.083, -2.076 }, 0.4, 1.2)); + // Negative blend radius + ASSERT_FALSE(executor_->optimoveL({ -0.203, 0.263, 0.559, 0.68, -1.083, -2.076 }, 0.4, 0.7, -0.1)); +} + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv); From 7577407b3d79fe401e35afc44c837e3bb4e41352 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 8 Jul 2025 08:05:08 +0200 Subject: [PATCH 02/12] Add missing file --- src/control/motion_primitives.cpp | 94 +++++++++++++++++++++++++++++++ 1 file changed, 94 insertions(+) create mode 100644 src/control/motion_primitives.cpp diff --git a/src/control/motion_primitives.cpp b/src/control/motion_primitives.cpp new file mode 100644 index 000000000..b4e208a65 --- /dev/null +++ b/src/control/motion_primitives.cpp @@ -0,0 +1,94 @@ +// -- 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 +#include + +namespace urcl::control +{ + +bool MotionPrimitive::validate() const +{ + if (acceleration < 0) + { + URCL_LOG_ERROR("Negative acceleration passed to motion primitive. This is not allowed."); + return false; + } + if (velocity < 0) + { + URCL_LOG_ERROR("Negative velocity passed to motion primitive. This is not allowed."); + return false; + } + if (blend_radius < 0) + { + URCL_LOG_ERROR("Negative blend radius passed to motion primitive. This is not allowed."); + return false; + } + return true; +} + +bool OptimoveJPrimitive::validate() const +{ + if (!MotionPrimitive::validate()) + { + return false; + } + if (acceleration <= 0 || acceleration > 1.0) + { + URCL_LOG_ERROR("Acceleration fraction must be in range (0, 1]."); + return false; + } + if (velocity <= 0 || velocity > 1.0) + { + URCL_LOG_ERROR("Velocity fraction must be in range (0, 1]."); + return false; + } + return true; +} + +bool OptimoveLPrimitive::validate() const +{ + if (!MotionPrimitive::validate()) + { + return false; + } + if (acceleration <= 0 || acceleration > 1.0) + { + URCL_LOG_ERROR("Acceleration fraction must be in range (0, 1]."); + return false; + } + if (velocity <= 0 || velocity > 1.0) + { + URCL_LOG_ERROR("Velocity fraction must be in range (0, 1]."); + return false; + } + return true; +} +} // namespace urcl::control \ No newline at end of file From afcf1a4cef2500638bafbddae9e31ce8fc165651 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 8 Jul 2025 08:12:43 +0200 Subject: [PATCH 03/12] Switch back default of headless mode in example --- examples/instruction_executor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/instruction_executor.cpp b/examples/instruction_executor.cpp index 229b9f02a..85fa5ade0 100644 --- a/examples/instruction_executor.cpp +++ b/examples/instruction_executor.cpp @@ -58,7 +58,7 @@ int main(int argc, char* argv[]) robot_ip = std::string(argv[1]); } - bool headless_mode = false; + bool headless_mode = true; g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode, "external_control.urp"); if (!g_my_robot->getUrDriver()->checkCalibration(CALIBRATION_CHECKSUM)) @@ -113,4 +113,4 @@ int main(int argc, char* argv[]) g_my_robot->getUrDriver()->stopControl(); return 0; -} +} \ No newline at end of file From eb0ae0582aaa03055b1cfee1467dc403c6f72695 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 8 Jul 2025 08:27:23 +0200 Subject: [PATCH 04/12] Fix motion primitive validation Have individual validation functions for each primitive type. --- .../control/motion_primitives.h | 12 +++- src/control/motion_primitives.cpp | 72 +++++++++++++++++-- 2 files changed, 79 insertions(+), 5 deletions(-) diff --git a/include/ur_client_library/control/motion_primitives.h b/include/ur_client_library/control/motion_primitives.h index ce21c200a..25511b329 100644 --- a/include/ur_client_library/control/motion_primitives.h +++ b/include/ur_client_library/control/motion_primitives.h @@ -63,6 +63,7 @@ enum class TrajectorySplineType : int32_t struct MotionPrimitive { + virtual ~MotionPrimitive() = default; MotionType type = MotionType::UNKNOWN; std::chrono::duration duration; double acceleration; @@ -86,6 +87,7 @@ struct MoveJPrimitive : public MotionPrimitive this->blend_radius = blend_radius; } + bool validate() const override; urcl::vector6d_t target_joint_configuration; }; @@ -103,6 +105,8 @@ struct MoveLPrimitive : public MotionPrimitive this->blend_radius = blend_radius; } + bool validate() const override; + urcl::Pose target_pose; }; @@ -117,6 +121,9 @@ struct MovePPrimitive : public MotionPrimitive this->velocity = velocity; this->blend_radius = blend_radius; } + + bool validate() const override; + urcl::Pose target_pose; }; @@ -133,6 +140,9 @@ struct MoveCPrimitive : public MotionPrimitive this->blend_radius = blend_radius; this->mode = mode; } + + bool validate() const override; + urcl::Pose via_point_pose; urcl::Pose target_pose; int32_t mode = 0; @@ -202,4 +212,4 @@ struct OptimoveLPrimitive : public MotionPrimitive }; } // namespace control } // namespace urcl -#endif // UR_CLIENT_LIBRARY_MOTION_PRIMITIVES_H_INCLUDED +#endif // UR_CLIENT_LIBRARY_MOTION_PRIMITIVES_H_INCLUDED \ No newline at end of file diff --git a/src/control/motion_primitives.cpp b/src/control/motion_primitives.cpp index b4e208a65..846807887 100644 --- a/src/control/motion_primitives.cpp +++ b/src/control/motion_primitives.cpp @@ -36,19 +36,83 @@ namespace urcl::control bool MotionPrimitive::validate() const { + if (blend_radius < 0) + { + URCL_LOG_ERROR("Negative blend radius passed to motion primitive. This is not allowed."); + return false; + } + return true; +} +bool MoveJPrimitive::validate() const +{ + if (!MotionPrimitive::validate()) + { + return false; + } if (acceleration < 0) { - URCL_LOG_ERROR("Negative acceleration passed to motion primitive. This is not allowed."); + URCL_LOG_ERROR("Negative acceleration passed to MoveJ primitive. This is not allowed."); return false; } if (velocity < 0) { - URCL_LOG_ERROR("Negative velocity passed to motion primitive. This is not allowed."); + URCL_LOG_ERROR("Negative velocity passed to MoveJ primitive. This is not allowed."); return false; } - if (blend_radius < 0) + return true; +} + +bool MoveLPrimitive::validate() const +{ + if (!MotionPrimitive::validate()) { - URCL_LOG_ERROR("Negative blend radius passed to motion primitive. This is not allowed."); + return false; + } + if (acceleration < 0) + { + URCL_LOG_ERROR("Negative acceleration passed to MoveL primitive. This is not allowed."); + return false; + } + if (velocity < 0) + { + URCL_LOG_ERROR("Negative velocity passed to MoveL primitive. This is not allowed."); + return false; + } + return true; +} +bool MovePPrimitive::validate() const +{ + if (!MotionPrimitive::validate()) + { + return false; + } + if (acceleration < 0) + { + URCL_LOG_ERROR("Negative acceleration passed to MoveP primitive. This is not allowed."); + return false; + } + if (velocity < 0) + { + URCL_LOG_ERROR("Negative velocity passed to MoveP primitive. This is not allowed."); + return false; + } + return true; +} + +bool MoveCPrimitive::validate() const +{ + if (!MotionPrimitive::validate()) + { + return false; + } + if (acceleration < 0) + { + URCL_LOG_ERROR("Negative acceleration passed to MoveC primitive. This is not allowed."); + return false; + } + if (velocity < 0) + { + URCL_LOG_ERROR("Negative velocity passed to MoveC primitive. This is not allowed."); return false; } return true; From de118df51228ecc116083406f8528aaf3a94d48b Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 8 Jul 2025 08:36:03 +0200 Subject: [PATCH 05/12] Initialize blend radius with 0 Otherwise that might trigger the negative check for motion primitives that don't explicitly set it. --- include/ur_client_library/control/motion_primitives.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/ur_client_library/control/motion_primitives.h b/include/ur_client_library/control/motion_primitives.h index 25511b329..4d767b414 100644 --- a/include/ur_client_library/control/motion_primitives.h +++ b/include/ur_client_library/control/motion_primitives.h @@ -68,7 +68,7 @@ struct MotionPrimitive std::chrono::duration duration; double acceleration; double velocity; - double blend_radius; + double blend_radius = 0.0; virtual bool validate() const; }; From 82a8bc7a3fc71edac87c93a481677c1245aecf95 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 8 Jul 2025 09:52:30 +0200 Subject: [PATCH 06/12] Update doc/examples/instruction_executor.rst Co-authored-by: Mads Holm Peters <79145214+urmahp@users.noreply.github.com> --- doc/examples/instruction_executor.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/examples/instruction_executor.rst b/doc/examples/instruction_executor.rst index a2f67a1cc..d9ca495c1 100644 --- a/doc/examples/instruction_executor.rst +++ b/doc/examples/instruction_executor.rst @@ -47,7 +47,7 @@ Each element in the motion sequence can be a different motion type. In the examp ``MoveJ`` motions, a ``MoveL`` motion, a ``MoveP`` motion, a ``OptimiveJ`` motion and a ``OptimoveL`` motion. The primitives' parameters are directly forwarded to the underlying script functions, so the parameter descriptions for them apply, as well. Particularly, you may want to -choose between either a time-based execution speed or an acceleration / velocity parametrization +choose between either a time-based execution or an acceleration / velocity parametrization for some move functions. The latter will be ignored if a time > 0 is given. Please refer to the script manual for details. From 8dcfe275de8039ebe00605994826b97f58d33e0f Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 8 Jul 2025 09:59:53 +0200 Subject: [PATCH 07/12] Merge validation functions Use the same validation function for most primitives, while providing a specialized validation function for spline primitives. --- .../control/motion_primitives.h | 14 ++-- src/control/motion_primitives.cpp | 65 ++----------------- 2 files changed, 10 insertions(+), 69 deletions(-) diff --git a/include/ur_client_library/control/motion_primitives.h b/include/ur_client_library/control/motion_primitives.h index 4d767b414..1da44a6de 100644 --- a/include/ur_client_library/control/motion_primitives.h +++ b/include/ur_client_library/control/motion_primitives.h @@ -87,7 +87,6 @@ struct MoveJPrimitive : public MotionPrimitive this->blend_radius = blend_radius; } - bool validate() const override; urcl::vector6d_t target_joint_configuration; }; @@ -105,8 +104,6 @@ struct MoveLPrimitive : public MotionPrimitive this->blend_radius = blend_radius; } - bool validate() const override; - urcl::Pose target_pose; }; @@ -122,8 +119,6 @@ struct MovePPrimitive : public MotionPrimitive this->blend_radius = blend_radius; } - bool validate() const override; - urcl::Pose target_pose; }; @@ -141,8 +136,6 @@ struct MoveCPrimitive : public MotionPrimitive this->mode = mode; } - bool validate() const override; - urcl::Pose via_point_pose; urcl::Pose target_pose; int32_t mode = 0; @@ -172,6 +165,9 @@ struct SplinePrimitive : public MotionPrimitive return control::TrajectorySplineType::SPLINE_CUBIC; } } + + bool validate() const override; + vector6d_t target_positions; vector6d_t target_velocities; std::optional target_accelerations; @@ -189,9 +185,9 @@ struct OptimoveJPrimitive : public MotionPrimitive this->velocity = velocity_fraction; } - urcl::vector6d_t target_joint_configuration; - bool validate() const override; + + urcl::vector6d_t target_joint_configuration; }; struct OptimoveLPrimitive : public MotionPrimitive diff --git a/src/control/motion_primitives.cpp b/src/control/motion_primitives.cpp index 846807887..3295f3502 100644 --- a/src/control/motion_primitives.cpp +++ b/src/control/motion_primitives.cpp @@ -41,83 +41,28 @@ bool MotionPrimitive::validate() const URCL_LOG_ERROR("Negative blend radius passed to motion primitive. This is not allowed."); return false; } - return true; -} -bool MoveJPrimitive::validate() const -{ - if (!MotionPrimitive::validate()) - { - return false; - } - if (acceleration < 0) - { - URCL_LOG_ERROR("Negative acceleration passed to MoveJ primitive. This is not allowed."); - return false; - } - if (velocity < 0) - { - URCL_LOG_ERROR("Negative velocity passed to MoveJ primitive. This is not allowed."); - return false; - } - return true; -} - -bool MoveLPrimitive::validate() const -{ - if (!MotionPrimitive::validate()) - { - return false; - } - if (acceleration < 0) - { - URCL_LOG_ERROR("Negative acceleration passed to MoveL primitive. This is not allowed."); - return false; - } - if (velocity < 0) - { - URCL_LOG_ERROR("Negative velocity passed to MoveL primitive. This is not allowed."); - return false; - } - return true; -} -bool MovePPrimitive::validate() const -{ - if (!MotionPrimitive::validate()) - { - return false; - } if (acceleration < 0) { - URCL_LOG_ERROR("Negative acceleration passed to MoveP primitive. This is not allowed."); + URCL_LOG_ERROR("Negative acceleration passed to motion primitive. This is not allowed."); return false; } if (velocity < 0) { - URCL_LOG_ERROR("Negative velocity passed to MoveP primitive. This is not allowed."); + URCL_LOG_ERROR("Negative velocity passed to motion primitive. This is not allowed."); return false; } return true; } -bool MoveCPrimitive::validate() const +bool SplinePrimitive::validate() const { - if (!MotionPrimitive::validate()) - { - return false; - } - if (acceleration < 0) - { - URCL_LOG_ERROR("Negative acceleration passed to MoveC primitive. This is not allowed."); - return false; - } - if (velocity < 0) + if (duration <= 0) { - URCL_LOG_ERROR("Negative velocity passed to MoveC primitive. This is not allowed."); + URCL_LOG_ERROR("Duration must be greater than zero."); return false; } return true; } - bool OptimoveJPrimitive::validate() const { if (!MotionPrimitive::validate()) From 71bef419de30ca6e95318c93e5bdc113cbfaa21e Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 8 Jul 2025 10:16:53 +0200 Subject: [PATCH 08/12] Fix duration check --- src/control/motion_primitives.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/control/motion_primitives.cpp b/src/control/motion_primitives.cpp index 3295f3502..d7668f5d6 100644 --- a/src/control/motion_primitives.cpp +++ b/src/control/motion_primitives.cpp @@ -56,7 +56,7 @@ bool MotionPrimitive::validate() const bool SplinePrimitive::validate() const { - if (duration <= 0) + if (duration <= std::chrono::milliseconds(0)) { URCL_LOG_ERROR("Duration must be greater than zero."); return false; From a68cc53e7ac0c6c1bc2803e9c9e15dd7c594920e Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 8 Jul 2025 11:19:08 +0200 Subject: [PATCH 09/12] Revert validation for splines Since zero times are allowed if it is the first point and the position is the current robot's position, we leave that check to the URScript --- src/control/motion_primitives.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/control/motion_primitives.cpp b/src/control/motion_primitives.cpp index d7668f5d6..89cc72171 100644 --- a/src/control/motion_primitives.cpp +++ b/src/control/motion_primitives.cpp @@ -56,11 +56,8 @@ bool MotionPrimitive::validate() const bool SplinePrimitive::validate() const { - if (duration <= std::chrono::milliseconds(0)) - { - URCL_LOG_ERROR("Duration must be greater than zero."); - return false; - } + // Spline primitives don't have the same restriction as others do. Whether the primitives are valid or not + // is checked in the URScript program. return true; } bool OptimoveJPrimitive::validate() const From e6c27c7d22e29a479665368e40405638bbc731a1 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 8 Jul 2025 13:08:35 +0200 Subject: [PATCH 10/12] Increase maximum workflow time --- .github/workflows/ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index e37421bbb..d371300f0 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -10,7 +10,7 @@ on: jobs: build: - timeout-minutes: 30 + timeout-minutes: 60 runs-on: ubuntu-latest name: build (${{matrix.env.URSIM_VERSION}}-${{matrix.env.ROBOT_MODEL}}) strategy: From aeee44e09701405893cbcc200046a47d50b35f84 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 8 Jul 2025 13:38:48 +0200 Subject: [PATCH 11/12] Make popups non-blocking Otherwise the CI will be hanging on older versions --- resources/external_control.urscript | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 02a902f8b..16db302d3 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -566,10 +566,10 @@ thread trajectoryThread(): {% elif ROBOT_SOFTWARE_VERSION >= v10.8.0 %} optimovej(q, a = acceleration, v = velocity, r = blend_radius) {% else %} - popup("Optimovej is only supported from software 10.8.0 and upwards.", error=True, blocking=True) + popup("Optimovej is only supported from software 10.8.0 and upwards.", error=True, blocking=False) {% endif %} {% else %} - popup("Optimovej is only supported from software 5.21.0 and upwards.", error=True, blocking=True) + popup("Optimovej is only supported from software 5.21.0 and upwards.", error=True, blocking=False) {% endif %} if DEBUG: @@ -595,10 +595,10 @@ thread trajectoryThread(): {% elif ROBOT_SOFTWARE_VERSION >= v10.8.0 %} optimovel(p[q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, r = blend_radius) {% else %} - popup("Optimovel is only supported from software 10.8.0 and upwards.", error=True, blocking=True) + popup("Optimovel is only supported from software 10.8.0 and upwards.", error=True, blocking=False) {% endif %} {% else %} - popup("Optimovel is only supported from software 5.21.0 and upwards.", error=True, blocking=True) + popup("Optimovel is only supported from software 5.21.0 and upwards.", error=True, blocking=False) {% endif %} if DEBUG: @@ -876,4 +876,4 @@ socket_close("reverse_socket") socket_close("trajectory_socket") socket_close("script_command_socket") -# NODE_CONTROL_LOOP_ENDS +# NODE_CONTROL_LOOP_ENDS \ No newline at end of file From d7ed7bcac9cf0c12f237409bd5db752f13d0d1cc Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 8 Jul 2025 14:24:00 +0200 Subject: [PATCH 12/12] Change Cartesian targets to fit for UR5e and UR20 In CI we use both, so we have to make sure the Cartesian target is reachable by both --- tests/test_instruction_executor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tests/test_instruction_executor.cpp b/tests/test_instruction_executor.cpp index 66cb07986..f83dfd627 100644 --- a/tests/test_instruction_executor.cpp +++ b/tests/test_instruction_executor.cpp @@ -415,8 +415,8 @@ TEST_F(InstructionExecutorTest, optimovel_succeeds) // move to a feasible starting pose ASSERT_TRUE(executor_->moveJ({ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 })); - ASSERT_TRUE(executor_->optimoveL({ -0.203, 0.263, 0.559, 0.68, -1.083, -2.076 }, 0.4, 0.7, 0.1)); - ASSERT_TRUE(executor_->optimoveL({ -0.203, 0.263, 0.459, 0.68, -1.083, -2.076 }, 1.0, 1.0, 0.1)); + ASSERT_TRUE(executor_->optimoveL({ -0.203, 0.463, 0.559, 0.68, -1.083, -2.076 }, 0.4, 0.7, 0.1)); + ASSERT_TRUE(executor_->optimoveL({ -0.203, 0.463, 0.459, 0.68, -1.083, -2.076 }, 1.0, 1.0, 0.1)); } TEST_F(InstructionExecutorTest, optimovej_with_illegal_parameters_fails)