From feba75ce6c6397c7221a9075cf839e7f2328da74 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 21 Apr 2026 15:35:27 +0200 Subject: [PATCH 01/33] WIP: Support pose and joint targets for movej and movel --- examples/instruction_executor.cpp | 14 ++++--- .../control/motion_primitives.h | 38 ++++++++++++++++++- .../ur/instruction_executor.h | 6 +++ resources/external_control.urscript | 22 +++++++++++ src/control/trajectory_point_interface.cpp | 20 ++++++++++ src/ur/instruction_executor.cpp | 17 ++++++++- 6 files changed, 110 insertions(+), 7 deletions(-) diff --git a/examples/instruction_executor.cpp b/examples/instruction_executor.cpp index 85fa5ade0..97ca41f5a 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)) @@ -89,6 +89,8 @@ int main(int argc, char* argv[]) std::make_shared(urcl::Pose(-0.203, 0.263, 0.559, 0.68, -1.083, -2.076), 0.1, std::chrono::seconds(2)), + std::make_shared(urcl::vector6d_t{ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 }, 0.1, + 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, @@ -96,21 +98,23 @@ int main(int argc, char* argv[]) 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); + // instruction_executor->executeMotion(motion_sequence); double goal_time_sec = 2.0; // acceleration / velocity parametrization - instruction_executor->moveJ({ -1.57, -1.57, 0, 0, 0, 0 }, 2.0, 2.0); + // instruction_executor->moveJ({ -1.57, -1.57, 0, 0, 0, 0 }, 2.0, 2.0); // goal time parametrization -- acceleration and velocity will be ignored instruction_executor->moveJ({ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 }, 0.1, 0.1, goal_time_sec); // acceleration / velocity parametrization instruction_executor->moveL({ -0.203, 0.263, 0.559, 0.68, -1.083, -2.076 }, 1.5, 1.5); // goal time parametrization -- acceleration and velocity will be ignored instruction_executor->moveL({ -0.203, 0.463, 0.559, 0.68, -1.083, -2.076 }, 0.1, 0.1, goal_time_sec); + instruction_executor->moveLToJointTarget({ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 }, 1.5, 1.5, goal_time_sec); + instruction_executor->moveJToPoseTarget({ -0.203, 0.463, 0.559, 0.68, -1.083, -2.076 }, 0.1, 0.1, goal_time_sec); - instruction_executor->moveP({ -0.203, 0.463, 0.759, 0.68, -1.083, -2.076 }, 1.5, 1.5); + // instruction_executor->moveP({ -0.203, 0.463, 0.759, 0.68, -1.083, -2.076 }, 1.5, 1.5); g_my_robot->getUrDriver()->stopControl(); return 0; -} \ No newline at end of file +} diff --git a/include/ur_client_library/control/motion_primitives.h b/include/ur_client_library/control/motion_primitives.h index 1da44a6de..80c253611 100644 --- a/include/ur_client_library/control/motion_primitives.h +++ b/include/ur_client_library/control/motion_primitives.h @@ -48,6 +48,8 @@ enum class MotionType : uint8_t MOVEC = 3, OPTIMOVEJ = 4, OPTIMOVEL = 5, + MOVEJ_POSE = 6, + MOVEL_JOINT = 7, SPLINE = 51, UNKNOWN = 255 }; @@ -90,6 +92,23 @@ struct MoveJPrimitive : public MotionPrimitive urcl::vector6d_t target_joint_configuration; }; +struct MoveJPosePrimitive : public MotionPrimitive +{ + MoveJPosePrimitive(const urcl::Pose& target, const double blend_radius = 0, + const std::chrono::duration duration = std::chrono::milliseconds(0), + const double acceleration = 1.4, const double velocity = 1.04) + { + type = MotionType::MOVEJ_POSE; + target_pose = target; + this->duration = duration; + this->acceleration = acceleration; + this->velocity = velocity; + this->blend_radius = blend_radius; + } + + urcl::Pose target_pose; +}; + struct MoveLPrimitive : public MotionPrimitive { MoveLPrimitive(const urcl::Pose& target, const double blend_radius = 0, @@ -107,6 +126,23 @@ struct MoveLPrimitive : public MotionPrimitive urcl::Pose target_pose; }; +struct MoveLJointPrimitive : public MotionPrimitive +{ + MoveLJointPrimitive(const urcl::vector6d_t& target, const double blend_radius = 0, + const std::chrono::duration duration = std::chrono::milliseconds(0), + const double acceleration = 1.4, const double velocity = 1.04) + { + type = MotionType::MOVEL_JOINT; + target_joint_configuration = target; + this->duration = duration; + this->acceleration = acceleration; + this->velocity = velocity; + this->blend_radius = blend_radius; + } + + urcl::vector6d_t target_joint_configuration; +}; + struct MovePPrimitive : public MotionPrimitive { MovePPrimitive(const urcl::Pose& target, const double blend_radius = 0, const double acceleration = 1.4, @@ -208,4 +244,4 @@ struct OptimoveLPrimitive : public MotionPrimitive }; } // namespace control } // namespace urcl -#endif // UR_CLIENT_LIBRARY_MOTION_PRIMITIVES_H_INCLUDED \ No newline at end of file +#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 f5fd28956..704dac47e 100644 --- a/include/ur_client_library/ur/instruction_executor.h +++ b/include/ur_client_library/ur/instruction_executor.h @@ -81,6 +81,9 @@ class InstructionExecutor bool moveJ(const urcl::vector6d_t& target, const double acceleration = 1.4, const double velocity = 1.04, const double time = 0, const double blend_radius = 0); + bool moveJToPoseTarget(const urcl::Pose& target, const double acceleration = 1.4, const double velocity = 1.04, + const double time = 0, const double blend_radius = 0); + /** * \brief Move the robot to a pose target using movel * @@ -98,6 +101,9 @@ class InstructionExecutor bool moveL(const urcl::Pose& target, const double acceleration = 1.4, const double velocity = 1.04, const double time = 0, const double blend_radius = 0); + bool moveLToJointTarget(const urcl::vector6d_t& target, const double acceleration = 1.4, const double velocity = 1.04, + const double time = 0, const double blend_radius = 0); + /** * \brief Move the robot to a pose target using movep * diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 2462f3517..b0ab0842a 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -40,6 +40,8 @@ MOTION_TYPE_MOVEP = 2 MOTION_TYPE_MOVEC = 3 MOTION_TYPE_OPTIMOVEJ = 4 MOTION_TYPE_OPTIMOVEL = 5 +MOTION_TYPE_MOVEJ_POSE = 6 +MOTION_TYPE_MOVEL_JOINT = 7 MOTION_TYPE_SPLINE = 51 TRAJECTORY_DATA_DIMENSION = 3 * 6 + 1 @@ -559,6 +561,7 @@ thread trajectoryThread(): end # MoveJ point if raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEJ: + textmsg("movej") acceleration = raw_point[13] / MULT_jointstate velocity = raw_point[7] / MULT_jointstate movej(q, a = acceleration, v = velocity, t = tmptime, r = blend_radius) @@ -566,13 +569,32 @@ thread trajectoryThread(): # reset old acceleration spline_qdd = [0, 0, 0, 0, 0, 0] spline_qd = [0, 0, 0, 0, 0, 0] + elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEJ_POSE: + textmsg("movej_pose") + acceleration = raw_point[13] / MULT_jointstate + velocity = raw_point[7] / MULT_jointstate + movej(p[q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, t = tmptime, r = blend_radius) + + # reset old acceleration + spline_qdd = [0, 0, 0, 0, 0, 0] + spline_qd = [0, 0, 0, 0, 0, 0] # Movel point elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEL: + textmsg("movel") acceleration = raw_point[13] / MULT_jointstate velocity = raw_point[7] / MULT_jointstate movel(p[q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, t = tmptime, r = blend_radius) + # reset old acceleration + spline_qdd = [0, 0, 0, 0, 0, 0] + spline_qd = [0, 0, 0, 0, 0, 0] + elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEL_JOINT: + textmsg("movel_q") + acceleration = raw_point[13] / MULT_jointstate + velocity = raw_point[7] / MULT_jointstate + movel([q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, t = tmptime, r = blend_radius) + # reset old acceleration spline_qdd = [0, 0, 0, 0, 0, 0] spline_qd = [0, 0, 0, 0, 0, 0] diff --git a/src/control/trajectory_point_interface.cpp b/src/control/trajectory_point_interface.cpp index 0e72d5209..d7e777c6e 100644 --- a/src/control/trajectory_point_interface.cpp +++ b/src/control/trajectory_point_interface.cpp @@ -96,6 +96,17 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptracceleration); break; } + case MotionType::MOVEJ_POSE: + { + auto movej_primitive = std::static_pointer_cast(primitive); + first_block = { + movej_primitive->target_pose.x, movej_primitive->target_pose.y, movej_primitive->target_pose.z, + movej_primitive->target_pose.rx, movej_primitive->target_pose.ry, movej_primitive->target_pose.rz + }; + second_block.fill(primitive->velocity); + third_block.fill(primitive->acceleration); + break; + } case MotionType::MOVEL: { auto movel_primitive = std::static_pointer_cast(primitive); @@ -107,6 +118,14 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptracceleration); break; } + case MotionType::MOVEL_JOINT: + { + auto movel_primitive = std::static_pointer_cast(primitive); + first_block = movel_primitive->target_joint_configuration; + second_block.fill(primitive->velocity); + third_block.fill(primitive->acceleration); + break; + } case MotionType::MOVEP: { auto movep_primitive = std::static_pointer_cast(primitive); @@ -163,6 +182,7 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptrtype); throw UnsupportedMotionType(); } diff --git a/src/ur/instruction_executor.cpp b/src/ur/instruction_executor.cpp index ab6243e31..722037dbb 100644 --- a/src/ur/instruction_executor.cpp +++ b/src/ur/instruction_executor.cpp @@ -71,11 +71,12 @@ bool urcl::InstructionExecutor::executeMotion( { try { + URCL_LOG_INFO("Sending motion primitive of type %d to robot", static_cast(primitive->type)); driver_->writeMotionPrimitive(primitive); } catch (const UnsupportedMotionType&) { - URCL_LOG_ERROR("Unsupported motion type"); + URCL_LOG_ERROR("Unsupported motion type %d", static_cast(primitive->type)); // The hardware will complain about missing trajectory points and return a failure for // trajectory execution. Hence, we need to step into the running loop below. } @@ -104,6 +105,13 @@ bool urcl::InstructionExecutor::moveJ(const urcl::vector6d_t& target, const doub target, blend_radius, std::chrono::milliseconds(static_cast(time * 1000)), acceleration, velocity) }); } +bool urcl::InstructionExecutor::moveJToPoseTarget(const urcl::Pose& target, const double acceleration, + const double velocity, const double time, const double blend_radius) +{ + 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) { @@ -111,6 +119,13 @@ bool urcl::InstructionExecutor::moveL(const urcl::Pose& target, const double acc target, blend_radius, std::chrono::milliseconds(static_cast(time * 1000)), acceleration, velocity) }); } +bool urcl::InstructionExecutor::moveLToJointTarget(const urcl::vector6d_t& target, const double acceleration, + const double velocity, const double time, const double blend_radius) +{ + return executeMotion({ std::make_shared( + target, blend_radius, std::chrono::milliseconds(static_cast(time * 1000)), acceleration, velocity) }); +} + bool urcl::InstructionExecutor::moveP(const urcl::Pose& target, const double acceleration, const double velocity, const double blend_radius) { From 9d28c4af80145e0081b9c62d833c74a69d5b3250 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 23 Apr 2026 11:24:38 +0200 Subject: [PATCH 02/33] Use a variant to enable equivalent syntax for movej and movel --- examples/instruction_executor.cpp | 13 ++-- .../control/motion_primitives.h | 62 ++++++++++++------- include/ur_client_library/exceptions.h | 9 +++ include/ur_client_library/types.h | 12 ++++ .../ur/instruction_executor.h | 9 ++- src/control/trajectory_point_interface.cpp | 54 +++++++++------- src/ur/instruction_executor.cpp | 13 ++-- 7 files changed, 106 insertions(+), 66 deletions(-) diff --git a/examples/instruction_executor.cpp b/examples/instruction_executor.cpp index 97ca41f5a..08871b88e 100644 --- a/examples/instruction_executor.cpp +++ b/examples/instruction_executor.cpp @@ -89,8 +89,8 @@ int main(int argc, char* argv[]) std::make_shared(urcl::Pose(-0.203, 0.263, 0.559, 0.68, -1.083, -2.076), 0.1, std::chrono::seconds(2)), - std::make_shared(urcl::vector6d_t{ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 }, 0.1, - std::chrono::seconds(2)), + std::make_shared(urcl::Q{ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 }, 0.1, + 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, @@ -106,12 +106,9 @@ int main(int argc, char* argv[]) // instruction_executor->moveJ({ -1.57, -1.57, 0, 0, 0, 0 }, 2.0, 2.0); // goal time parametrization -- acceleration and velocity will be ignored instruction_executor->moveJ({ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 }, 0.1, 0.1, goal_time_sec); - // acceleration / velocity parametrization - instruction_executor->moveL({ -0.203, 0.263, 0.559, 0.68, -1.083, -2.076 }, 1.5, 1.5); - // goal time parametrization -- acceleration and velocity will be ignored - instruction_executor->moveL({ -0.203, 0.463, 0.559, 0.68, -1.083, -2.076 }, 0.1, 0.1, goal_time_sec); - instruction_executor->moveLToJointTarget({ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 }, 1.5, 1.5, goal_time_sec); - instruction_executor->moveJToPoseTarget({ -0.203, 0.463, 0.559, 0.68, -1.083, -2.076 }, 0.1, 0.1, goal_time_sec); + instruction_executor->moveL({ -0.0203, 0.363, 0.559, 0.68, -1.083, -2.076 }, 1.5, 1.5); + instruction_executor->moveL(urcl::Q{ -1.572, -1.686, 1.707, -0.833, 0.782, 0.479 }, 1.5, 1.5, goal_time_sec); + instruction_executor->moveJ(urcl::Pose{ -0.0203, 0.363, 0.559, 0.68, -1.083, -2.076 }, 0.1, 0.1, goal_time_sec); // instruction_executor->moveP({ -0.203, 0.463, 0.759, 0.68, -1.083, -2.076 }, 1.5, 1.5); diff --git a/include/ur_client_library/control/motion_primitives.h b/include/ur_client_library/control/motion_primitives.h index 80c253611..8233e0fc2 100644 --- a/include/ur_client_library/control/motion_primitives.h +++ b/include/ur_client_library/control/motion_primitives.h @@ -33,6 +33,7 @@ #include #include +#include #include namespace urcl @@ -88,24 +89,33 @@ struct MoveJPrimitive : public MotionPrimitive this->velocity = velocity; this->blend_radius = blend_radius; } - - urcl::vector6d_t target_joint_configuration; -}; - -struct MoveJPosePrimitive : public MotionPrimitive -{ - MoveJPosePrimitive(const urcl::Pose& target, const double blend_radius = 0, - const std::chrono::duration duration = std::chrono::milliseconds(0), - const double acceleration = 1.4, const double velocity = 1.04) + MoveJPrimitive(const urcl::MotionTarget& target, const double blend_radius = 0, + const std::chrono::duration duration = std::chrono::milliseconds(0), + const double acceleration = 1.4, const double velocity = 1.04) { - type = MotionType::MOVEJ_POSE; - target_pose = target; this->duration = duration; this->acceleration = acceleration; this->velocity = velocity; this->blend_radius = blend_radius; + + std::visit( + [&](const auto& target_variant) { + using T = std::decay_t; + if constexpr (std::is_same_v) + { + type = MotionType::MOVEJ_POSE; + target_pose = target_variant; + } + else if constexpr (std::is_same_v) + { + type = MotionType::MOVEJ; + target_joint_configuration = target_variant.values; + } + }, + target); } + urcl::vector6d_t target_joint_configuration; urcl::Pose target_pose; }; @@ -122,24 +132,32 @@ struct MoveLPrimitive : public MotionPrimitive this->velocity = velocity; this->blend_radius = blend_radius; } - - urcl::Pose target_pose; -}; - -struct MoveLJointPrimitive : public MotionPrimitive -{ - MoveLJointPrimitive(const urcl::vector6d_t& target, const double blend_radius = 0, - const std::chrono::duration duration = std::chrono::milliseconds(0), - const double acceleration = 1.4, const double velocity = 1.04) + MoveLPrimitive(const urcl::MotionTarget& target, const double blend_radius = 0, + const std::chrono::duration duration = std::chrono::milliseconds(0), + const double acceleration = 1.4, const double velocity = 1.04) { - type = MotionType::MOVEL_JOINT; - target_joint_configuration = target; this->duration = duration; this->acceleration = acceleration; this->velocity = velocity; this->blend_radius = blend_radius; + std::visit( + [&](const auto& target_variant) { + using T = std::decay_t; + if constexpr (std::is_same_v) + { + type = MotionType::MOVEL; + target_pose = target_variant; + } + else if constexpr (std::is_same_v) + { + type = MotionType::MOVEL_JOINT; + target_joint_configuration = target_variant.values; + } + }, + target); } + urcl::Pose target_pose; urcl::vector6d_t target_joint_configuration; }; diff --git a/include/ur_client_library/exceptions.h b/include/ur_client_library/exceptions.h index 9554a80ce..6a6fbdc56 100644 --- a/include/ur_client_library/exceptions.h +++ b/include/ur_client_library/exceptions.h @@ -217,6 +217,15 @@ class UnsupportedMotionType : public UrException } }; +class InvalidData : public UrException +{ +public: + explicit InvalidData() = delete; + explicit InvalidData(const std::string& error_text) : std::runtime_error(error_text) + { + } +}; + class UnknownVariable : public UrException { private: diff --git a/include/ur_client_library/types.h b/include/ur_client_library/types.h index 861925772..c1ae2a829 100644 --- a/include/ur_client_library/types.h +++ b/include/ur_client_library/types.h @@ -24,6 +24,7 @@ #include #include #include +#include #include "ur_client_library/log.h" namespace urcl @@ -33,6 +34,15 @@ using vector6d_t = std::array; using vector6int32_t = std::array; using vector6uint32_t = std::array; +struct Q +{ + constexpr Q(double q1, double q2, double q3, double q4, double q5, double q6) : values{ q1, q2, q3, q4, q5, q6 } + { + } + + vector6d_t values; +}; + struct Pose { Pose() : x(0.0), y(0.0), z(0.0), rx(0.0), ry(0.0), rz(0.0) @@ -55,6 +65,8 @@ struct Pose } }; +using MotionTarget = std::variant; + template std::ostream& operator<<(std::ostream& out, const std::array& item) { diff --git a/include/ur_client_library/ur/instruction_executor.h b/include/ur_client_library/ur/instruction_executor.h index 704dac47e..2dae0fe5b 100644 --- a/include/ur_client_library/ur/instruction_executor.h +++ b/include/ur_client_library/ur/instruction_executor.h @@ -81,8 +81,8 @@ class InstructionExecutor bool moveJ(const urcl::vector6d_t& target, const double acceleration = 1.4, const double velocity = 1.04, const double time = 0, const double blend_radius = 0); - bool moveJToPoseTarget(const urcl::Pose& target, const double acceleration = 1.4, const double velocity = 1.04, - const double time = 0, const double blend_radius = 0); + bool moveJ(const MotionTarget& target, const double acceleration = 1.4, const double velocity = 1.04, + const double time = 0, const double blend_radius = 0); /** * \brief Move the robot to a pose target using movel @@ -100,9 +100,8 @@ class InstructionExecutor */ bool moveL(const urcl::Pose& target, const double acceleration = 1.4, const double velocity = 1.04, const double time = 0, const double blend_radius = 0); - - bool moveLToJointTarget(const urcl::vector6d_t& target, const double acceleration = 1.4, const double velocity = 1.04, - const double time = 0, const double blend_radius = 0); + bool moveL(const MotionTarget& target, const double acceleration = 1.4, const double velocity = 1.04, + const double time = 0, const double blend_radius = 0); /** * \brief Move the robot to a pose target using movep diff --git a/src/control/trajectory_point_interface.cpp b/src/control/trajectory_point_interface.cpp index d7e777c6e..2b442158b 100644 --- a/src/control/trajectory_point_interface.cpp +++ b/src/control/trajectory_point_interface.cpp @@ -89,39 +89,45 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptrtype) { case MotionType::MOVEJ: - { - auto movej_primitive = std::static_pointer_cast(primitive); - first_block = movej_primitive->target_joint_configuration; - second_block.fill(primitive->velocity); - third_block.fill(primitive->acceleration); - break; - } case MotionType::MOVEJ_POSE: { - auto movej_primitive = std::static_pointer_cast(primitive); - first_block = { - movej_primitive->target_pose.x, movej_primitive->target_pose.y, movej_primitive->target_pose.z, - movej_primitive->target_pose.rx, movej_primitive->target_pose.ry, movej_primitive->target_pose.rz - }; + auto movej_primitive = std::static_pointer_cast(primitive); + if (movej_primitive->type == MotionType::MOVEJ) + { + first_block = movej_primitive->target_joint_configuration; + } + else if (movej_primitive->type == MotionType::MOVEJ_POSE) + { + first_block = { movej_primitive->target_pose.x, movej_primitive->target_pose.y, + movej_primitive->target_pose.z, movej_primitive->target_pose.rx, + movej_primitive->target_pose.ry, movej_primitive->target_pose.rz }; + } + else + { + throw InvalidData("Motion type does not match motion primitive type."); + } second_block.fill(primitive->velocity); third_block.fill(primitive->acceleration); break; } case MotionType::MOVEL: - { - auto movel_primitive = std::static_pointer_cast(primitive); - first_block = { - movel_primitive->target_pose.x, movel_primitive->target_pose.y, movel_primitive->target_pose.z, - movel_primitive->target_pose.rx, movel_primitive->target_pose.ry, movel_primitive->target_pose.rz - }; - second_block.fill(primitive->velocity); - third_block.fill(primitive->acceleration); - break; - } case MotionType::MOVEL_JOINT: { - auto movel_primitive = std::static_pointer_cast(primitive); - first_block = movel_primitive->target_joint_configuration; + auto movel_primitive = std::static_pointer_cast(primitive); + if (movel_primitive->type == MotionType::MOVEL) + { + first_block = { movel_primitive->target_pose.x, movel_primitive->target_pose.y, + movel_primitive->target_pose.z, movel_primitive->target_pose.rx, + movel_primitive->target_pose.ry, movel_primitive->target_pose.rz }; + } + else if (movel_primitive->type == MotionType::MOVEL_JOINT) + { + first_block = movel_primitive->target_joint_configuration; + } + else + { + throw InvalidData("Motion type does not match motion primitive type."); + } second_block.fill(primitive->velocity); third_block.fill(primitive->acceleration); break; diff --git a/src/ur/instruction_executor.cpp b/src/ur/instruction_executor.cpp index 722037dbb..40be83fb3 100644 --- a/src/ur/instruction_executor.cpp +++ b/src/ur/instruction_executor.cpp @@ -105,10 +105,10 @@ bool urcl::InstructionExecutor::moveJ(const urcl::vector6d_t& target, const doub target, blend_radius, std::chrono::milliseconds(static_cast(time * 1000)), acceleration, velocity) }); } -bool urcl::InstructionExecutor::moveJToPoseTarget(const urcl::Pose& target, const double acceleration, - const double velocity, const double time, const double blend_radius) +bool urcl::InstructionExecutor::moveJ(const MotionTarget& target, const double acceleration, const double velocity, + const double time, const double blend_radius) { - return executeMotion({ std::make_shared( + return executeMotion({ std::make_shared( target, blend_radius, std::chrono::milliseconds(static_cast(time * 1000)), acceleration, velocity) }); } @@ -118,11 +118,10 @@ bool urcl::InstructionExecutor::moveL(const urcl::Pose& target, const double acc return executeMotion({ std::make_shared( target, blend_radius, std::chrono::milliseconds(static_cast(time * 1000)), acceleration, velocity) }); } - -bool urcl::InstructionExecutor::moveLToJointTarget(const urcl::vector6d_t& target, const double acceleration, - const double velocity, const double time, const double blend_radius) +bool urcl::InstructionExecutor::moveL(const MotionTarget& target, const double acceleration, const double velocity, + const double time, const double blend_radius) { - return executeMotion({ std::make_shared( + return executeMotion({ std::make_shared( target, blend_radius, std::chrono::milliseconds(static_cast(time * 1000)), acceleration, velocity) }); } From ebee9e2e589fc51add29ae3ce5247560ef17c199 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 23 Apr 2026 11:54:47 +0200 Subject: [PATCH 03/33] Remove debug statements --- resources/external_control.urscript | 4 ---- src/control/trajectory_point_interface.cpp | 1 - src/ur/instruction_executor.cpp | 1 - 3 files changed, 6 deletions(-) diff --git a/resources/external_control.urscript b/resources/external_control.urscript index b0ab0842a..79d5b7489 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -561,7 +561,6 @@ thread trajectoryThread(): end # MoveJ point if raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEJ: - textmsg("movej") acceleration = raw_point[13] / MULT_jointstate velocity = raw_point[7] / MULT_jointstate movej(q, a = acceleration, v = velocity, t = tmptime, r = blend_radius) @@ -570,7 +569,6 @@ thread trajectoryThread(): spline_qdd = [0, 0, 0, 0, 0, 0] spline_qd = [0, 0, 0, 0, 0, 0] elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEJ_POSE: - textmsg("movej_pose") acceleration = raw_point[13] / MULT_jointstate velocity = raw_point[7] / MULT_jointstate movej(p[q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, t = tmptime, r = blend_radius) @@ -581,7 +579,6 @@ thread trajectoryThread(): # Movel point elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEL: - textmsg("movel") acceleration = raw_point[13] / MULT_jointstate velocity = raw_point[7] / MULT_jointstate movel(p[q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, t = tmptime, r = blend_radius) @@ -590,7 +587,6 @@ thread trajectoryThread(): spline_qdd = [0, 0, 0, 0, 0, 0] spline_qd = [0, 0, 0, 0, 0, 0] elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEL_JOINT: - textmsg("movel_q") acceleration = raw_point[13] / MULT_jointstate velocity = raw_point[7] / MULT_jointstate movel([q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, t = tmptime, r = blend_radius) diff --git a/src/control/trajectory_point_interface.cpp b/src/control/trajectory_point_interface.cpp index 2b442158b..2dde0d271 100644 --- a/src/control/trajectory_point_interface.cpp +++ b/src/control/trajectory_point_interface.cpp @@ -188,7 +188,6 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptrtype); throw UnsupportedMotionType(); } diff --git a/src/ur/instruction_executor.cpp b/src/ur/instruction_executor.cpp index 40be83fb3..72ecc07ec 100644 --- a/src/ur/instruction_executor.cpp +++ b/src/ur/instruction_executor.cpp @@ -71,7 +71,6 @@ bool urcl::InstructionExecutor::executeMotion( { try { - URCL_LOG_INFO("Sending motion primitive of type %d to robot", static_cast(primitive->type)); driver_->writeMotionPrimitive(primitive); } catch (const UnsupportedMotionType&) From 282d10a38d6067b0159939f7be6e6f8f6040a716 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 24 Apr 2026 11:30:35 +0200 Subject: [PATCH 04/33] Implement MotionTargets for all motion types --- examples/instruction_executor.cpp | 34 ++++- .../control/motion_primitives.h | 134 ++++++++++++++++++ .../ur/instruction_executor.h | 8 ++ resources/external_control.urscript | 65 +++++++-- src/control/trajectory_point_interface.cpp | 79 +++++++++-- src/ur/instruction_executor.cpp | 24 ++++ 6 files changed, 315 insertions(+), 29 deletions(-) diff --git a/examples/instruction_executor.cpp b/examples/instruction_executor.cpp index 08871b88e..64d1e8db3 100644 --- a/examples/instruction_executor.cpp +++ b/examples/instruction_executor.cpp @@ -98,20 +98,44 @@ int main(int argc, char* argv[]) 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); + instruction_executor->executeMotion(motion_sequence); double goal_time_sec = 2.0; - // acceleration / velocity parametrization - // instruction_executor->moveJ({ -1.57, -1.57, 0, 0, 0, 0 }, 2.0, 2.0); + // acceleration / velocity parametrization brace-init style will be interpreted as joint + // positions + instruction_executor->moveJ({ -1.57, -1.57, 0, 0, 0, 0 }, 2.0, 2.0); + // goal time parametrization -- acceleration and velocity will be ignored instruction_executor->moveJ({ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 }, 0.1, 0.1, goal_time_sec); + + // moveL calls with brace-init style is interpreted as a pose instruction_executor->moveL({ -0.0203, 0.363, 0.559, 0.68, -1.083, -2.076 }, 1.5, 1.5); + // A pose can also be explicitly passed to moveL + instruction_executor->moveL(urcl::Pose{ -0.0203, 0.363, 0.559, 0.68, -1.083, -2.076 }, 1.5, 1.5); + // moveL can also accept a joint position target, if explicitly wrapped into a urcl::Q object instruction_executor->moveL(urcl::Q{ -1.572, -1.686, 1.707, -0.833, 0.782, 0.479 }, 1.5, 1.5, goal_time_sec); + + // moveJ can also accept a Cartesian pose, when given explicitly instruction_executor->moveJ(urcl::Pose{ -0.0203, 0.363, 0.559, 0.68, -1.083, -2.076 }, 0.1, 0.1, goal_time_sec); - // instruction_executor->moveP({ -0.203, 0.463, 0.759, 0.68, -1.083, -2.076 }, 1.5, 1.5); + // moveP can also be called with brace-init (interpreted as pose) or explicitly using a Pose or Q + instruction_executor->moveP({ -0.2, 0.363, 0.559, 0.68, -1.083, -2.076 }, 0.1, 0.1, goal_time_sec); + instruction_executor->moveP(urcl::Pose{ -0.0203, 0.303, 0.559, 0.68, -1.083, -2.076 }, 0.1, 0.1, goal_time_sec); + instruction_executor->moveP(urcl::Q{ -1.57, -1.83, 1.707, -0.833, 0.782, 0.479 }, 0.1, 0.1, goal_time_sec); + + // For moveC via and target can be a Pose or Q. When brace-init style lists are given, values are + // interpreted as Pose. + instruction_executor->moveC(urcl::Pose{ -0.1, 0.463, 0.559, 0.68, -1.083, -2.076 }, + urcl::Pose{ -0.0203, 0.303, 0.559, 0.68, -1.083, -2.076 }, 0.1, 0.1, goal_time_sec); + instruction_executor->moveC(urcl::Pose{ -0.1, 0.463, 0.559, 0.68, -1.083, -2.076 }, + urcl::Q{ -1.57, -1.83, 1.707, -0.833, 0.782, 0.479 }, 0.1, 0.1, goal_time_sec); + + // For optimove functions, the same target rules as for moveJ and moveL apply. + instruction_executor->optimoveJ({ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 }, 1.0, 1.0); + instruction_executor->optimoveL(urcl::Pose{ -0.0203, 0.363, 0.559, 0.68, -1.083, -2.076 }, 1.0, 1.0); + instruction_executor->optimoveL(urcl::Q{ -1.572, -1.686, 1.707, -0.833, 0.782, 0.479 }, 1.0, 1.0); + instruction_executor->optimoveJ(urcl::Pose{ -0.0203, 0.363, 0.559, 0.68, -1.083, -2.076 }, 1.0, 1.0); - g_my_robot->getUrDriver()->stopControl(); return 0; } diff --git a/include/ur_client_library/control/motion_primitives.h b/include/ur_client_library/control/motion_primitives.h index 8233e0fc2..1f21f0330 100644 --- a/include/ur_client_library/control/motion_primitives.h +++ b/include/ur_client_library/control/motion_primitives.h @@ -34,6 +34,7 @@ #include #include #include +#include "ur_client_library/exceptions.h" #include namespace urcl @@ -51,6 +52,12 @@ enum class MotionType : uint8_t OPTIMOVEL = 5, MOVEJ_POSE = 6, MOVEL_JOINT = 7, + MOVEP_JOINT = 8, + MOVEC_JOINT = 9, + MOVEC_JOINT_POSE = 10, + MOVEC_POSE_JOINT = 11, + OPTIMOVEJ_POSE = 12, + OPTIMOVEL_JOINT = 13, SPLINE = 51, UNKNOWN = 255 }; @@ -172,8 +179,31 @@ struct MovePPrimitive : public MotionPrimitive this->velocity = velocity; this->blend_radius = blend_radius; } + MovePPrimitive(const MotionTarget& target, const double blend_radius = 0, const double acceleration = 1.4, + const double velocity = 1.04) + { + std::visit( + [&](const auto& target_variant) { + using T = std::decay_t; + if constexpr (std::is_same_v) + { + type = MotionType::MOVEP; + target_pose = target_variant; + } + else if constexpr (std::is_same_v) + { + type = MotionType::MOVEP_JOINT; + target_joint_configuration = target_variant.values; + } + }, + target); + this->acceleration = acceleration; + this->velocity = velocity; + this->blend_radius = blend_radius; + } urcl::Pose target_pose; + urcl::vector6d_t target_joint_configuration; }; struct MoveCPrimitive : public MotionPrimitive @@ -190,8 +220,65 @@ struct MoveCPrimitive : public MotionPrimitive this->mode = mode; } + MoveCPrimitive(const MotionTarget& via_point, const MotionTarget& target, const double blend_radius = 0, + const double acceleration = 1.4, const double velocity = 1.04, const int32_t mode = 0) + { + if (std::holds_alternative(via_point)) + { + if (std::holds_alternative(target)) + { + type = MotionType::MOVEC_JOINT; + via_point_pose = urcl::Pose(); + target_pose = urcl::Pose(); + via_point_joint_configuration = std::get(via_point).values; + target_joint_configuration = std::get(target).values; + } + else if (std::holds_alternative(target)) + { + type = MotionType::MOVEC_POSE_JOINT; + via_point_pose = urcl::Pose(); + target_pose = std::get(target); + via_point_joint_configuration = std::get(via_point).values; + } + else + { + throw urcl::UrException("Unhandled motion target type for target point passed to MoveCPrimitive constructor"); + } + } + else if (std::holds_alternative(via_point)) + { + if (std::holds_alternative(target)) + { + type = MotionType::MOVEC_JOINT_POSE; + via_point_pose = std::get(via_point); + target_pose = urcl::Pose(); + target_joint_configuration = std::get(target).values; + } + else if (std::holds_alternative(target)) + { + type = MotionType::MOVEC; + via_point_pose = std::get(via_point); + target_pose = std::get(target); + } + else + { + throw urcl::UrException("Unhandled motion target type for target point passed to MoveCPrimitive constructor"); + } + } + else + { + throw urcl::UrException("Unhandled motion target type for via_point passed to MoveCPrimitive constructor"); + } + this->acceleration = acceleration; + this->velocity = velocity; + this->blend_radius = blend_radius; + this->mode = mode; + } + urcl::Pose via_point_pose; urcl::Pose target_pose; + urcl::vector6d_t via_point_joint_configuration; + urcl::vector6d_t target_joint_configuration; int32_t mode = 0; }; @@ -239,9 +326,33 @@ struct OptimoveJPrimitive : public MotionPrimitive this->velocity = velocity_fraction; } + OptimoveJPrimitive(const MotionTarget& target, const double blend_radius = 0, + const double acceleration_fraction = 0.3, const double velocity_fraction = 0.3) + { + std::visit( + [&](const auto& target_variant) { + using T = std::decay_t; + if constexpr (std::is_same_v) + { + type = MotionType::OPTIMOVEJ_POSE; + target_pose = target_variant; + } + else if constexpr (std::is_same_v) + { + type = MotionType::OPTIMOVEJ; + target_joint_configuration = target_variant.values; + } + }, + target); + this->blend_radius = blend_radius; + this->acceleration = acceleration_fraction; + this->velocity = velocity_fraction; + } + bool validate() const override; urcl::vector6d_t target_joint_configuration; + Pose target_pose; }; struct OptimoveLPrimitive : public MotionPrimitive @@ -255,10 +366,33 @@ struct OptimoveLPrimitive : public MotionPrimitive this->acceleration = acceleration_fraction; this->velocity = velocity_fraction; } + OptimoveLPrimitive(const MotionTarget& target, const double blend_radius = 0, + const double acceleration_fraction = 0.3, const double velocity_fraction = 0.3) + { + std::visit( + [&](const auto& target_variant) { + using T = std::decay_t; + if constexpr (std::is_same_v) + { + type = MotionType::OPTIMOVEL; + target_pose = target_variant; + } + else if constexpr (std::is_same_v) + { + type = MotionType::OPTIMOVEL_JOINT; + target_joint_configuration = target_variant.values; + } + }, + target); + this->blend_radius = blend_radius; + this->acceleration = acceleration_fraction; + this->velocity = velocity_fraction; + } bool validate() const override; urcl::Pose target_pose; + vector6d_t target_joint_configuration; }; } // namespace control } // namespace urcl diff --git a/include/ur_client_library/ur/instruction_executor.h b/include/ur_client_library/ur/instruction_executor.h index 2dae0fe5b..e0c48a0c9 100644 --- a/include/ur_client_library/ur/instruction_executor.h +++ b/include/ur_client_library/ur/instruction_executor.h @@ -118,6 +118,8 @@ class InstructionExecutor */ bool moveP(const urcl::Pose& target, const double acceleration = 1.4, const double velocity = 1.04, const double blend_radius = 0.0); + bool moveP(const MotionTarget& target, const double acceleration = 1.4, const double velocity = 1.04, + const double blend_radius = 0.0); /** * \brief Move the robot to a pose target using movec @@ -136,6 +138,8 @@ 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); + bool moveC(const MotionTarget& via, const MotionTarget& 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. @@ -154,6 +158,8 @@ class InstructionExecutor */ bool optimoveJ(const urcl::vector6d_t& target, const double acceleration_fraction = 0.3, const double velocity_fraction = 0.3, const double blend_radius = 0); + bool optimoveJ(const MotionTarget& 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. @@ -172,6 +178,8 @@ class InstructionExecutor */ bool optimoveL(const urcl::Pose& target, const double acceleration_fraction = 0.3, const double velocity_fraction = 0.3, const double blend_radius = 0); + bool optimoveL(const MotionTarget& target, const double acceleration_fraction = 0.3, + const double velocity_fraction = 0.3, const double blend_radius = 0); /** * \brief Cancel the current motion. diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 79d5b7489..319605cee 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -42,6 +42,12 @@ MOTION_TYPE_OPTIMOVEJ = 4 MOTION_TYPE_OPTIMOVEL = 5 MOTION_TYPE_MOVEJ_POSE = 6 MOTION_TYPE_MOVEL_JOINT = 7 +MOTION_TYPE_MOVEP_JOINT = 8 +MOTION_TYPE_MOVEC_JOINT = 9 +MOTION_TYPE_MOVEC_JOINT_POSE = 10 +MOTION_TYPE_MOVEC_POSE_JOINT = 11 +MOTION_TYPE_OPTIMOVEJ_POSE = 12 +MOTION_TYPE_OPTIMOVEL_JOINT = 13 MOTION_TYPE_SPLINE = 51 TRAJECTORY_DATA_DIMENSION = 3 * 6 + 1 @@ -604,11 +610,36 @@ thread trajectoryThread(): # reset old acceleration spline_qdd = [0, 0, 0, 0, 0, 0] spline_qd = [0, 0, 0, 0, 0, 0] + elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEP_JOINT: + acceleration = raw_point[13] / MULT_jointstate + velocity = raw_point[7] / MULT_jointstate + movep(q, a = acceleration, v = velocity, r = blend_radius) - elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEC: + # reset old acceleration + spline_qdd = [0, 0, 0, 0, 0, 0] + spline_qd = [0, 0, 0, 0, 0, 0] + + elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEC or + raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEC_JOINT or + raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEC_POSE_JOINT or + raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEC_JOINT_POSE: local v = [raw_point[7], raw_point[8], raw_point[9], raw_point[10], raw_point[11], raw_point[12]] / MULT_jointstate - via = p[v[0], v[1], v[2], v[3], v[4], v[5]] - target = p[q[0], q[1], q[2], q[3], q[4], q[5]] + if raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEC: + via = p[v[0], v[1], v[2], v[3], v[4], v[5]] + target = p[q[0], q[1], q[2], q[3], q[4], q[5]] + elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEC_JOINT: + via = v + target = q + elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEC_POSE_JOINT: + via = v + target = p[q[0], q[1], q[2], q[3], q[4], q[5]] + elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEC_JOINT_POSE: + via = p[v[0], v[1], v[2], v[3], v[4], v[5]] + target = q + end + + textmsg(str_cat(str_cat("MOVEC via: ", via), str_cat(" target: ", target))) + acceleration = raw_point[13] / MULT_jointstate velocity = raw_point[14] / MULT_jointstate mode = raw_point[15] / MULT_jointstate @@ -643,14 +674,22 @@ thread trajectoryThread(): end # OptimoveJ point - elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEJ: + elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEJ or raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEJ_POSE: 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) + if raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEJ: + optimovej(q, a = acceleration, v = velocity, r = blend_radius) + else: + optimovej(p[q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, r = blend_radius) + end {% elif ROBOT_SOFTWARE_VERSION >= v10.8.0 %} - optimovej(q, a = acceleration, v = velocity, r = blend_radius) + if raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEJ: + optimovej(q, a = acceleration, v = velocity, r = blend_radius) + else: + optimovej(p[q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, r = blend_radius) + end {% else %} popup("Optimovej is only supported from software 10.8.0 and upwards.", error=True, blocking=False) {% endif %} @@ -671,15 +710,23 @@ thread trajectoryThread(): spline_qd = [0, 0, 0, 0, 0, 0] # OptimoveL point - elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEL: + elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEL or raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEL_JOINT: 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) + if raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEL: + optimovel(p[q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, r = blend_radius) + else: + optimovel(q, a = acceleration, v = velocity, r = blend_radius) + end {% 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) + if raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEL: + optimovel(p[q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, r = blend_radius) + else: + optimovel(q, a = acceleration, v = velocity, r = blend_radius) + end {% else %} popup("Optimovel is only supported from software 10.8.0 and upwards.", error=True, blocking=False) {% endif %} diff --git a/src/control/trajectory_point_interface.cpp b/src/control/trajectory_point_interface.cpp index 2dde0d271..db45c6957 100644 --- a/src/control/trajectory_point_interface.cpp +++ b/src/control/trajectory_point_interface.cpp @@ -133,26 +133,57 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptr(primitive); - first_block = { - movep_primitive->target_pose.x, movep_primitive->target_pose.y, movep_primitive->target_pose.z, - movep_primitive->target_pose.rx, movep_primitive->target_pose.ry, movep_primitive->target_pose.rz - }; + if (movep_primitive->type == MotionType::MOVEP) + { + first_block = { movep_primitive->target_pose.x, movep_primitive->target_pose.y, + movep_primitive->target_pose.z, movep_primitive->target_pose.rx, + movep_primitive->target_pose.ry, movep_primitive->target_pose.rz }; + } + else if (movep_primitive->type == MotionType::MOVEP_JOINT) + { + first_block = movep_primitive->target_joint_configuration; + } second_block.fill(primitive->velocity); third_block.fill(primitive->acceleration); break; } case MotionType::MOVEC: + case MotionType::MOVEC_JOINT: + case MotionType::MOVEC_POSE_JOINT: + case MotionType::MOVEC_JOINT_POSE: { auto movec_primitive = std::static_pointer_cast(primitive); - first_block = { - movec_primitive->target_pose.x, movec_primitive->target_pose.y, movec_primitive->target_pose.z, - movec_primitive->target_pose.rx, movec_primitive->target_pose.ry, movec_primitive->target_pose.rz - }; - second_block = { movec_primitive->via_point_pose.x, movec_primitive->via_point_pose.y, - movec_primitive->via_point_pose.z, movec_primitive->via_point_pose.rx, - movec_primitive->via_point_pose.ry, movec_primitive->via_point_pose.rz }; + if (movec_primitive->type == MotionType::MOVEC) + { + first_block = { movec_primitive->target_pose.x, movec_primitive->target_pose.y, + movec_primitive->target_pose.z, movec_primitive->target_pose.rx, + movec_primitive->target_pose.ry, movec_primitive->target_pose.rz }; + second_block = { movec_primitive->via_point_pose.x, movec_primitive->via_point_pose.y, + movec_primitive->via_point_pose.z, movec_primitive->via_point_pose.rx, + movec_primitive->via_point_pose.ry, movec_primitive->via_point_pose.rz }; + } + else if (movec_primitive->type == MotionType::MOVEC_JOINT) + { + first_block = movec_primitive->target_joint_configuration; + second_block = movec_primitive->via_point_joint_configuration; + } + else if (movec_primitive->type == MotionType::MOVEC_POSE_JOINT) + { + first_block = { movec_primitive->target_pose.x, movec_primitive->target_pose.y, + movec_primitive->target_pose.z, movec_primitive->target_pose.rx, + movec_primitive->target_pose.ry, movec_primitive->target_pose.rz }; + second_block = movec_primitive->via_point_joint_configuration; + } + else if (movec_primitive->type == MotionType::MOVEC_JOINT_POSE) + { + first_block = movec_primitive->target_joint_configuration; + second_block = { movec_primitive->via_point_pose.x, movec_primitive->via_point_pose.y, + movec_primitive->via_point_pose.z, movec_primitive->via_point_pose.rx, + movec_primitive->via_point_pose.ry, movec_primitive->via_point_pose.rz }; + } third_block = { primitive->velocity, primitive->acceleration, static_cast(movec_primitive->mode), 0, 0, 0 }; @@ -170,19 +201,37 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptr(primitive); - first_block = optimovej_primitive->target_joint_configuration; + if (optimovej_primitive->type == MotionType::OPTIMOVEJ) + { + first_block = optimovej_primitive->target_joint_configuration; + } + else if (optimovej_primitive->type == MotionType::OPTIMOVEJ_POSE) + { + first_block = { optimovej_primitive->target_pose.x, optimovej_primitive->target_pose.y, + optimovej_primitive->target_pose.z, optimovej_primitive->target_pose.rx, + optimovej_primitive->target_pose.ry, optimovej_primitive->target_pose.rz }; + } second_block.fill(primitive->velocity); third_block.fill(primitive->acceleration); break; } case control::MotionType::OPTIMOVEL: + case control::MotionType::OPTIMOVEL_JOINT: { 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 }; + if (optimovel_primitive->type == MotionType::OPTIMOVEL) + { + 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 }; + } + else if (optimovel_primitive->type == MotionType::OPTIMOVEL_JOINT) + { + first_block = optimovel_primitive->target_joint_configuration; + } second_block.fill(primitive->velocity); third_block.fill(primitive->acceleration); break; diff --git a/src/ur/instruction_executor.cpp b/src/ur/instruction_executor.cpp index 72ecc07ec..c959000c6 100644 --- a/src/ur/instruction_executor.cpp +++ b/src/ur/instruction_executor.cpp @@ -129,6 +129,11 @@ bool urcl::InstructionExecutor::moveP(const urcl::Pose& target, const double acc { return executeMotion({ std::make_shared(target, blend_radius, acceleration, velocity) }); } +bool urcl::InstructionExecutor::moveP(const MotionTarget& target, const double acceleration, const double velocity, + const double blend_radius) +{ + return executeMotion({ std::make_shared(target, blend_radius, acceleration, velocity) }); +} bool urcl::InstructionExecutor::moveC(const urcl::Pose& via, const urcl::Pose& target, const double acceleration, const double velocity, const double blend_radius, const int32_t mode) @@ -137,18 +142,37 @@ 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::moveC(const MotionTarget& via, const MotionTarget& target, const double acceleration, + const double velocity, const double blend_radius, const int32_t mode) +{ + return executeMotion( + { 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::optimoveJ(const MotionTarget& 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::optimoveL(const MotionTarget& 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; From 793ac4629c2053eb7289b172c3ef32eacd81a987 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 24 Apr 2026 11:52:41 +0200 Subject: [PATCH 05/33] Revert headless_mode=false --- doc/architecture/instruction_executor.rst | 16 ++++++++++++++++ examples/instruction_executor.cpp | 2 +- 2 files changed, 17 insertions(+), 1 deletion(-) diff --git a/doc/architecture/instruction_executor.rst b/doc/architecture/instruction_executor.rst index c143d983f..44169945b 100644 --- a/doc/architecture/instruction_executor.rst +++ b/doc/architecture/instruction_executor.rst @@ -16,6 +16,22 @@ to point motions easily accessible. Currently, it supports the following instruc * Execute OptimoveL point to point motions (For PolyScope 5.21 / PolyScope 10.8 and later) * Execute sequences consisting of the motion primitives above +Joint and Cartesian targets +--------------------------- + +Every motion function comes in two flavours: + +* A "native" overload whose parameter is the target type that matches the underlying URScript + command (``vector6d_t`` for ``moveJ`` / ``optimoveJ``, ``urcl::Pose`` for ``moveL``, ``moveP``, + ``moveC`` and ``optimoveL``). Braced-initializer-list calls such as + ``moveJ({ q1, q2, q3, q4, q5, q6 })`` bind to this overload and keep the behaviour from older + releases. +* A ``urcl::MotionTarget`` overload that accepts either a ``urcl::Q`` (joint configuration) or a + ``urcl::Pose`` (Cartesian pose). This lets the same function perform a motion whose target type + does not match the URScript command's natural argument, e.g. ``moveJ(urcl::Pose{...})`` to reach + a Cartesian target with a joint-interpolated motion, or ``moveL(urcl::Q{...})`` to perform a + linear tool-space motion towards the pose implied by a joint configuration. + The Instruction Executor uses the :ref:`trajectory_point_interface` and the :ref:`reverse_interface` for sending motion instructions to the robot. Hence, it requires a :ref:`ur_driver` object. diff --git a/examples/instruction_executor.cpp b/examples/instruction_executor.cpp index 64d1e8db3..f37b5de96 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)) From a2f5484720126a7b4ab969e8a8a9487a4b56869d Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 24 Apr 2026 14:41:51 +0200 Subject: [PATCH 06/33] Updated documentation and tests --- .../trajectory_point_interface.rst | 71 +++- doc/examples/instruction_executor.rst | 23 +- .../control/motion_primitives.h | 89 ++++- include/ur_client_library/exceptions.h | 7 + include/ur_client_library/types.h | 25 ++ .../ur/instruction_executor.h | 118 +++++++ resources/external_control.urscript | 2 - tests/test_instruction_executor.cpp | 94 ++++++ tests/test_trajectory_point_interface.cpp | 312 +++++++++++++++--- 9 files changed, 654 insertions(+), 87 deletions(-) diff --git a/doc/architecture/trajectory_point_interface.rst b/doc/architecture/trajectory_point_interface.rst index 08a0d0990..b22cf8f34 100644 --- a/doc/architecture/trajectory_point_interface.rst +++ b/doc/architecture/trajectory_point_interface.rst @@ -41,28 +41,53 @@ representations in 21 datafields. The data fields have the following meaning: ===== ===== index meaning ===== ===== - 0-5 trajectory point positions (Multiplied by ``MULT_JOINTSTATE``) - 6-11 trajectory point velocities (Multiplied by ``MULT_JOINTSTATE``). For MOVEC, this contains the "via pose". - 12-17 trajectory point accelerations (Multiplied by ``MULT_JOINTSTATE``). - - For MOVEC: - - - 12: velocity (Multiplied by ``MULT_JOINTSTATE``) - - 13: acceleration (Multiplied by ``MULT_JOINTSTATE``) - - 14: mode (Multiplied by ``MULT_JOINTSTATE``) - - 18 trajectory point type - - - 0: MOVEJ - - 1: MOVEL - - 2: MOVEP - - 3: MOVEC - - 51: SPLINE) + 0-5 trajectory point positions (multiplied by ``MULT_JOINTSTATE``). + + Interpreted as joint positions [rad] or as a Cartesian pose ([m, m, m, rad, rad, rad]) + depending on the motion type at index 18 (see below). + + 6-11 trajectory point velocities (multiplied by ``MULT_JOINTSTATE``). + + For all MOVEC variants this field is repurposed to carry the via point (same + joint-vs-pose interpretation as the target at indices 0-5, see the motion type at + index 18). + + 12-17 trajectory point accelerations (multiplied by ``MULT_JOINTSTATE``). + + For all MOVEC variants: + + - 12: velocity (multiplied by ``MULT_JOINTSTATE``) + - 13: acceleration (multiplied by ``MULT_JOINTSTATE``) + - 14: mode (multiplied by ``MULT_JOINTSTATE``) + + 18 trajectory point type. The base values below use the URScript command's "natural" + target type (joints for ``movej`` / ``optimovej``, Cartesian pose for ``movel`` / + ``movep`` / ``movec`` / ``optimovel``). The ``*_POSE`` / ``*_JOINT`` variants indicate + that the other target kind is being sent instead. + + - 0: MOVEJ – ``movej`` to a joint target + - 1: MOVEL – ``movel`` to a pose target + - 2: MOVEP – ``movep`` to a pose target + - 3: MOVEC – ``movec`` with pose via and pose target + - 4: OPTIMOVEJ – ``optimovej`` to a joint target + - 5: OPTIMOVEL – ``optimovel`` to a pose target + - 6: MOVEJ_POSE – ``movej`` to a pose target (IK on the robot controller) + - 7: MOVEL_JOINT – ``movel`` to the pose implied by a joint target (FK on the + robot controller) + - 8: MOVEP_JOINT – ``movep`` to the pose implied by a joint target (FK on the + robot controller) + - 9: MOVEC_JOINT – ``movec`` with joint via and joint target + - 10: MOVEC_JOINT_POSE – ``movec`` with pose via and joint target + - 11: MOVEC_POSE_JOINT – ``movec`` with joint via and pose target + - 12: OPTIMOVEJ_POSE – ``optimovej`` to a pose target + - 13: OPTIMOVEL_JOINT – ``optimovel`` to the pose implied by a joint target + - 51: SPLINE 19 trajectory point time (in seconds, multiplied by ``MULT_TIME``) 20 depending on trajectory point type - - MOVEJ, MOVEL, MOVEP and MOVEC: point blend radius (in meters, multiplied by ``MULT_TIME``) + - All MOVE* and OPTIMOVE* variants: point blend radius (in meters, multiplied by + ``MULT_TIME``) - SPLINE: spline type (1: CUBIC, 2: QUINTIC) ===== ===== @@ -75,3 +100,13 @@ where With ``MULT_TIME`` being 1000000, the maximum duration that can be sent is 2147 seconds, while precision is cut off at 1 microsecond. (The same applies to the blend radius, respectively being max 2147 m and 1 μm precision.) + +.. note:: + The ``*_POSE`` / ``*_JOINT`` motion-type variants let callers mix joint-space and Cartesian + targets freely through the high-level APIs (see :ref:`instruction_executor` and the + ``urcl::MotionTarget`` type). On the wire the positions are always packed as a 6-tuple of + ``MULT_JOINTSTATE``-scaled integers; which physical quantity they represent (joint angles or + Cartesian pose components) is determined solely by the motion type field at index 18. The + corresponding mapping back to ``movej`` / ``movel`` / ``movep`` / ``movec`` / ``optimovej`` / + ``optimovel`` calls with either ``q`` or ``p[...]`` arguments is performed on the robot side + by ``resources/external_control.urscript``. diff --git a/doc/examples/instruction_executor.rst b/doc/examples/instruction_executor.rst index d9ca495c1..084b9fbed 100644 --- a/doc/examples/instruction_executor.rst +++ b/doc/examples/instruction_executor.rst @@ -50,13 +50,20 @@ functions, so the parameter descriptions for them apply, as well. Particularly, 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. +Each motion primitive can be constructed either with its "native" target type (``vector6d_t`` for +``MoveJ`` / ``OptimoveJ``, ``urcl::Pose`` for ``MoveL`` / ``MoveP`` / ``MoveC`` / ``OptimoveL``) +or with a ``urcl::MotionTarget``, which can hold either a ``urcl::Q`` (joint configuration) or a +``urcl::Pose`` (Cartesian pose). This makes it possible, for example, to use a +``MoveLPrimitive`` with a joint configuration – the robot will then compute the forward +kinematics on the controller and linearly interpolate in tool space towards the resulting pose. + Please refer to the script manual for details. Execute a single motion ----------------------- -To run a single motion, the ``InstructionExecutor`` provides the methods ``moveJ(...)`` and -``moveL(...)``: +To run a single motion, the ``InstructionExecutor`` provides the methods ``moveJ(...)``, +``moveL(...)``, ``moveP(...)``, ``moveC(...)``, ``optimoveJ(...)`` and ``optimoveL(...)``: .. literalinclude:: ../../examples/instruction_executor.cpp :language: c++ @@ -64,6 +71,16 @@ To run a single motion, the ``InstructionExecutor`` provides the methods ``moveJ :linenos: :lineno-match: :start-at: double goal_time_sec = 2.0; - :end-before: g_my_robot->getUrDriver()->stopControl(); + :end-before: return 0; Again, time parametrization has priority over acceleration / velocity parameters. + +Each motion function has two overloads: one taking the "natural" target type +(``vector6d_t`` for ``moveJ`` / ``optimoveJ``, ``urcl::Pose`` for the other motions), and one +taking a ``urcl::MotionTarget``. Passing a braced-initializer-list of six doubles selects the +natural overload, so ``moveJ({ ... })`` continues to be interpreted as joint positions and +``moveL({ ... })`` as a Cartesian pose. To select the other interpretation, wrap the values in +``urcl::Q{...}`` or ``urcl::Pose{...}`` explicitly – for example, +``moveJ(urcl::Pose{...})`` performs a ``movej`` towards a Cartesian target and +``moveL(urcl::Q{...})`` performs a ``movel`` towards the pose implied by a joint configuration. +The same rules apply to ``moveP``, ``moveC``, ``optimoveJ`` and ``optimoveL``. diff --git a/include/ur_client_library/control/motion_primitives.h b/include/ur_client_library/control/motion_primitives.h index 1f21f0330..99d407622 100644 --- a/include/ur_client_library/control/motion_primitives.h +++ b/include/ur_client_library/control/motion_primitives.h @@ -42,22 +42,36 @@ namespace urcl namespace control { +/*! + * \brief Discriminator for the motion primitive type sent over the trajectory interface. + * + * The base values (``MOVEJ``, ``MOVEL``, ``MOVEP``, ``MOVEC``, ``OPTIMOVEJ``, ``OPTIMOVEL``) use + * their "natural" target type for the URScript command (joint configuration for ``movej`` / + * ``optimovej``, Cartesian pose for ``movel`` / ``movep`` / ``movec`` / ``optimovel``). The + * additional ``*_POSE`` and ``*_JOINT`` entries indicate that the *other* target kind was + * requested, e.g. ``MOVEJ_POSE`` performs a ``movej`` towards a Cartesian pose, and + * ``MOVEC_POSE_JOINT`` performs a ``movec`` whose via point is a Cartesian pose and whose target + * is a joint configuration. + * + * These values must stay in sync with the ``MOTION_TYPE_*`` constants in + * ``resources/external_control.urscript``. + */ enum class MotionType : uint8_t { - MOVEJ = 0, - MOVEL = 1, - MOVEP = 2, - MOVEC = 3, - OPTIMOVEJ = 4, - OPTIMOVEL = 5, - MOVEJ_POSE = 6, - MOVEL_JOINT = 7, - MOVEP_JOINT = 8, - MOVEC_JOINT = 9, - MOVEC_JOINT_POSE = 10, - MOVEC_POSE_JOINT = 11, - OPTIMOVEJ_POSE = 12, - OPTIMOVEL_JOINT = 13, + MOVEJ = 0, //!< ``movej`` towards a joint configuration. + MOVEL = 1, //!< ``movel`` towards a Cartesian pose. + MOVEP = 2, //!< ``movep`` towards a Cartesian pose. + MOVEC = 3, //!< ``movec`` with via and target as Cartesian poses. + OPTIMOVEJ = 4, //!< ``optimovej`` towards a joint configuration. + OPTIMOVEL = 5, //!< ``optimovel`` towards a Cartesian pose. + MOVEJ_POSE = 6, //!< ``movej`` towards a Cartesian pose. + MOVEL_JOINT = 7, //!< ``movel`` towards a joint configuration. + MOVEP_JOINT = 8, //!< ``movep`` towards a joint configuration. + MOVEC_JOINT = 9, //!< ``movec`` with via and target both as joint configurations. + MOVEC_JOINT_POSE = 10, //!< ``movec`` with a Cartesian via and a joint target. + MOVEC_POSE_JOINT = 11, //!< ``movec`` with a joint via and a Cartesian target. + OPTIMOVEJ_POSE = 12, //!< ``optimovej`` towards a Cartesian pose. + OPTIMOVEL_JOINT = 13, //!< ``optimovel`` towards a joint configuration. SPLINE = 51, UNKNOWN = 255 }; @@ -96,6 +110,14 @@ struct MoveJPrimitive : public MotionPrimitive this->velocity = velocity; this->blend_radius = blend_radius; } + /*! + * \brief Construct a MoveJ primitive from a \ref urcl::MotionTarget. + * + * If ``target`` holds a \ref urcl::Q, ``type`` is set to \ref MotionType::MOVEJ and + * ``target_joint_configuration`` is populated. If ``target`` holds a \ref urcl::Pose, ``type`` + * is set to \ref MotionType::MOVEJ_POSE and ``target_pose`` is populated instead; the robot + * will internally solve inverse kinematics to reach the pose with a ``movej``. + */ MoveJPrimitive(const urcl::MotionTarget& target, const double blend_radius = 0, const std::chrono::duration duration = std::chrono::milliseconds(0), const double acceleration = 1.4, const double velocity = 1.04) @@ -139,6 +161,15 @@ struct MoveLPrimitive : public MotionPrimitive this->velocity = velocity; this->blend_radius = blend_radius; } + /*! + * \brief Construct a MoveL primitive from a \ref urcl::MotionTarget. + * + * If ``target`` holds a \ref urcl::Pose, ``type`` is set to \ref MotionType::MOVEL. If it + * holds a \ref urcl::Q, ``type`` is set to \ref MotionType::MOVEL_JOINT and the configuration + * is stored in ``target_joint_configuration``. The robot will still execute a tool-space + * linear motion, resolving the joint configuration to its forward kinematics pose on the + * controller. + */ MoveLPrimitive(const urcl::MotionTarget& target, const double blend_radius = 0, const std::chrono::duration duration = std::chrono::milliseconds(0), const double acceleration = 1.4, const double velocity = 1.04) @@ -179,6 +210,12 @@ struct MovePPrimitive : public MotionPrimitive this->velocity = velocity; this->blend_radius = blend_radius; } + /*! + * \brief Construct a MoveP primitive from a \ref urcl::MotionTarget. + * + * Analogous to \ref MoveJPrimitive / \ref MoveLPrimitive: a \ref urcl::Pose selects + * \ref MotionType::MOVEP, a \ref urcl::Q selects \ref MotionType::MOVEP_JOINT. + */ MovePPrimitive(const MotionTarget& target, const double blend_radius = 0, const double acceleration = 1.4, const double velocity = 1.04) { @@ -220,6 +257,18 @@ struct MoveCPrimitive : public MotionPrimitive this->mode = mode; } + /*! + * \brief Construct a MoveC primitive from two \ref urcl::MotionTarget values. + * + * Every combination of \ref urcl::Pose and \ref urcl::Q for the via point and the target is + * supported and mapped to the corresponding \ref MotionType (``MOVEC``, + * ``MOVEC_JOINT``, ``MOVEC_POSE_JOINT``, or ``MOVEC_JOINT_POSE``). When a joint configuration + * is used for either role, only the corresponding joint member is populated and the pose + * member is set to a default-constructed \ref urcl::Pose. + * + * \throws urcl::UrException if either variant is ever extended with an alternative that is + * not handled here. + */ MoveCPrimitive(const MotionTarget& via_point, const MotionTarget& target, const double blend_radius = 0, const double acceleration = 1.4, const double velocity = 1.04, const int32_t mode = 0) { @@ -326,6 +375,12 @@ struct OptimoveJPrimitive : public MotionPrimitive this->velocity = velocity_fraction; } + /*! + * \brief Construct an OptimoveJ primitive from a \ref urcl::MotionTarget. + * + * A \ref urcl::Q selects \ref MotionType::OPTIMOVEJ, a \ref urcl::Pose selects + * \ref MotionType::OPTIMOVEJ_POSE. + */ OptimoveJPrimitive(const MotionTarget& target, const double blend_radius = 0, const double acceleration_fraction = 0.3, const double velocity_fraction = 0.3) { @@ -366,6 +421,12 @@ struct OptimoveLPrimitive : public MotionPrimitive this->acceleration = acceleration_fraction; this->velocity = velocity_fraction; } + /*! + * \brief Construct an OptimoveL primitive from a \ref urcl::MotionTarget. + * + * A \ref urcl::Pose selects \ref MotionType::OPTIMOVEL, a \ref urcl::Q selects + * \ref MotionType::OPTIMOVEL_JOINT. + */ OptimoveLPrimitive(const MotionTarget& target, const double blend_radius = 0, const double acceleration_fraction = 0.3, const double velocity_fraction = 0.3) { diff --git a/include/ur_client_library/exceptions.h b/include/ur_client_library/exceptions.h index 6a6fbdc56..1dda3c78d 100644 --- a/include/ur_client_library/exceptions.h +++ b/include/ur_client_library/exceptions.h @@ -217,6 +217,13 @@ class UnsupportedMotionType : public UrException } }; +/*! + * \brief Thrown when data received from or passed through the library does not match the + * expected structure or value range. + * + * Examples include motion targets whose variant alternative is not handled by the corresponding + * primitive constructor. + */ class InvalidData : public UrException { public: diff --git a/include/ur_client_library/types.h b/include/ur_client_library/types.h index c1ae2a829..bcf3f7a0c 100644 --- a/include/ur_client_library/types.h +++ b/include/ur_client_library/types.h @@ -34,6 +34,17 @@ using vector6d_t = std::array; using vector6int32_t = std::array; using vector6uint32_t = std::array; +/*! + * \brief A joint configuration (6 joint positions in radians). + * + * This is a strong type around a \ref vector6d_t meant to unambiguously express "this 6-tuple + * represents joint values", as opposed to a Cartesian pose. It is primarily used together with + * \ref MotionTarget to select between joint-space and Cartesian-space targets when calling + * motion functions that can accept either. + * + * Unlike raw initializer lists (``{...}``) which may bind to either \ref vector6d_t or + * \ref Pose, wrapping values in ``urcl::Q{...}`` always forces a joint-target interpretation. + */ struct Q { constexpr Q(double q1, double q2, double q3, double q4, double q5, double q6) : values{ q1, q2, q3, q4, q5, q6 } @@ -65,6 +76,20 @@ struct Pose } }; +/*! + * \brief A tagged union representing either a joint target (\ref Q) or a Cartesian target + * (\ref Pose). + * + * ``MotionTarget`` is used throughout the motion API (e.g. + * \ref InstructionExecutor::moveJ "InstructionExecutor::moveJ" and the ``Move*Primitive`` + * constructors) to let callers choose at call site whether a motion should be parametrized in + * joint space or in Cartesian space without needing separate overloads for every combination. + * + * The overloads that take a ``MotionTarget`` are provided alongside explicit \ref vector6d_t and + * \ref Pose overloads so that plain braced-initializer calls keep binding to the previous + * behaviour; only explicitly constructed \ref Q or \ref Pose values (or an already-built + * ``MotionTarget``) select the variant-based path. + */ using MotionTarget = std::variant; template diff --git a/include/ur_client_library/ur/instruction_executor.h b/include/ur_client_library/ur/instruction_executor.h index e0c48a0c9..8669b5ec1 100644 --- a/include/ur_client_library/ur/instruction_executor.h +++ b/include/ur_client_library/ur/instruction_executor.h @@ -81,6 +81,28 @@ class InstructionExecutor bool moveJ(const urcl::vector6d_t& target, const double acceleration = 1.4, const double velocity = 1.04, const double time = 0, const double blend_radius = 0); + /** + * \brief Move the robot to a joint or Cartesian target using movej. + * + * This overload accepts a \ref urcl::MotionTarget which can hold either a \ref urcl::Q (joint + * target, forwarded as ``movej(q, ...)``) or a \ref urcl::Pose (Cartesian target, forwarded as + * ``movej(p[...], ...)``). In the latter case the robot will internally solve inverse + * kinematics. The robot will move with the given acceleration and velocity. The function will + * return once the robot has reached the target. + * + * \note A braced-initializer-list of six doubles (``moveJ({...}, ...)``) will bind to the + * \ref vector6d_t overload above, not to this overload. To call this overload with a pose, pass + * an explicit \ref urcl::Pose; with a joint target, pass an explicit \ref urcl::Q or use the + * vector6d_t overload. + * + * \param target The joint or Cartesian target to move to. + * \param acceleration Joint acceleration of leading axis [rad/s^2] + * \param velocity Joint speed of leading axis [rad/s] + * \param time The time to reach the target. If set to 0, the robot will move with the given acceleration and + * velocity. + * \param blend_radius The blend radius to use for the motion. + * \return True if the robot has reached the target, false otherwise. + */ bool moveJ(const MotionTarget& target, const double acceleration = 1.4, const double velocity = 1.04, const double time = 0, const double blend_radius = 0); @@ -100,6 +122,27 @@ class InstructionExecutor */ bool moveL(const urcl::Pose& target, const double acceleration = 1.4, const double velocity = 1.04, const double time = 0, const double blend_radius = 0); + + /** + * \brief Move the robot to a Cartesian or joint target using movel. + * + * This overload accepts a \ref urcl::MotionTarget. A \ref urcl::Pose is forwarded as + * ``movel(p[...], ...)``; a \ref urcl::Q is forwarded as ``movel(q, ...)`` (the robot + * internally runs forward kinematics to perform a linear motion in tool space towards the joint + * configuration's resulting pose). + * + * \note A braced-initializer-list of six doubles (``moveL({...}, ...)``) will bind to the + * \ref urcl::Pose overload above. To target a joint configuration, pass an explicit + * \ref urcl::Q. + * + * \param target The target to move to. + * \param acceleration Tool acceleration [m/s^2] + * \param velocity Tool speed [m/s] + * \param time The time to reach the target. If set to 0, the robot will move with the given acceleration and + * velocity. + * \param blend_radius The blend radius to use for the motion. + * \return True if the robot has reached the target, false otherwise. + */ bool moveL(const MotionTarget& target, const double acceleration = 1.4, const double velocity = 1.04, const double time = 0, const double blend_radius = 0); @@ -118,6 +161,24 @@ class InstructionExecutor */ bool moveP(const urcl::Pose& target, const double acceleration = 1.4, const double velocity = 1.04, const double blend_radius = 0.0); + + /** + * \brief Move the robot to a Cartesian or joint target using movep. + * + * This overload accepts a \ref urcl::MotionTarget. A \ref urcl::Pose is forwarded as + * ``movep(p[...], ...)``; a \ref urcl::Q is forwarded as ``movep(q, ...)``. + * + * \note A braced-initializer-list of six doubles (``moveP({...}, ...)``) will bind to the + * \ref urcl::Pose overload above. To target a joint configuration, pass an explicit + * \ref urcl::Q. + * + * \param target The target to move to. + * \param acceleration Tool acceleration [m/s^2] + * \param velocity Tool speed [m/s] + * \param blend_radius The blend radius to use for the motion. + * + * \return True if the robot has reached the target, false otherwise. + */ bool moveP(const MotionTarget& target, const double acceleration = 1.4, const double velocity = 1.04, const double blend_radius = 0.0); @@ -138,6 +199,27 @@ 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 in a circular motion using movec. + * + * This overload accepts \ref urcl::MotionTarget values for both the via point and the target. + * Each of them can independently be a \ref urcl::Pose (passed as ``p[...]``) or a + * \ref urcl::Q (passed as a joint configuration, translated into a pose by the controller's + * forward kinematics before executing the circular motion). + * + * \note Braced-initializer-lists of six doubles will bind to the \ref urcl::Pose overload above. + * Wrap with an explicit \ref urcl::Q to target joint configurations. + * + * \param via The via point defining the circle. + * \param target The target to move to. + * \param acceleration Tool acceleration [m/s^2] + * \param velocity Tool speed [m/s] + * \param blend_radius The blend radius to use for the motion. + * \param mode The movec mode as defined in the URScript manual. + * + * \return True if the robot has reached the target, false otherwise. + */ bool moveC(const MotionTarget& via, const MotionTarget& target, const double acceleration = 1.4, const double velocity = 1.04, const double blend_radius = 0.0, const int32_t mode = 0); @@ -158,6 +240,24 @@ class InstructionExecutor */ 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 joint or Cartesian target using optimoveJ. + * + * This overload accepts a \ref urcl::MotionTarget. A \ref urcl::Q is forwarded as a joint + * target; a \ref urcl::Pose is forwarded as a Cartesian target (``optimovej(p[...], ...)``). + * + * \note A braced-initializer-list of six doubles binds to the \ref vector6d_t overload above. + * + * \param target The 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 + * (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 MotionTarget& target, const double acceleration_fraction = 0.3, const double velocity_fraction = 0.3, const double blend_radius = 0); @@ -178,6 +278,24 @@ class InstructionExecutor */ bool optimoveL(const urcl::Pose& target, const double acceleration_fraction = 0.3, const double velocity_fraction = 0.3, const double blend_radius = 0); + + /** + * \brief Move the robot to a Cartesian or joint target using optimoveL. + * + * This overload accepts a \ref urcl::MotionTarget. A \ref urcl::Pose is forwarded as a + * Cartesian target; a \ref urcl::Q is forwarded as a joint target (``optimovel(q, ...)``). + * + * \note A braced-initializer-list of six doubles binds to the \ref urcl::Pose overload above. + * + * \param target The 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 + * (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 MotionTarget& target, const double acceleration_fraction = 0.3, const double velocity_fraction = 0.3, const double blend_radius = 0); diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 319605cee..371225d88 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -638,8 +638,6 @@ thread trajectoryThread(): target = q end - textmsg(str_cat(str_cat("MOVEC via: ", via), str_cat(" target: ", target))) - acceleration = raw_point[13] / MULT_jointstate velocity = raw_point[14] / MULT_jointstate mode = raw_point[15] / MULT_jointstate diff --git a/tests/test_instruction_executor.cpp b/tests/test_instruction_executor.cpp index 9efdccd34..cc3cb4a0a 100644 --- a/tests/test_instruction_executor.cpp +++ b/tests/test_instruction_executor.cpp @@ -456,6 +456,100 @@ TEST_F(InstructionExecutorTest, optimovel_with_illegal_parameters_fails) ASSERT_FALSE(executor_->optimoveL({ -0.203, 0.263, 0.559, 0.68, -1.083, -2.076 }, 0.4, 0.7, -0.1)); } +TEST_F(InstructionExecutorTest, movej_accepts_pose_via_motion_target) +{ + // move to a feasible starting pose + ASSERT_TRUE(executor_->moveJ({ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 })); + + // Using an explicit urcl::Pose selects the MotionTarget overload and triggers a MOVEJ_POSE + // execution on the robot. + ASSERT_TRUE(executor_->moveJ(urcl::Pose{ -0.203, 0.363, 0.559, 0.68, -1.083, -2.076 }, 1.0, 1.0)); +} + +TEST_F(InstructionExecutorTest, movej_accepts_joint_via_motion_target) +{ + // Wrapping joint positions in urcl::Q selects the MotionTarget overload and triggers a normal + // MOVEJ execution. + ASSERT_TRUE(executor_->moveJ(urcl::Q{ -1.57, -1.57, 0, 0, 0, 0 }, 2.0, 2.0)); +} + +TEST_F(InstructionExecutorTest, movel_accepts_joint_via_motion_target) +{ + // move to a feasible starting pose + ASSERT_TRUE(executor_->moveJ({ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 })); + + // urcl::Q selects the MotionTarget overload and executes a MOVEL_JOINT: linear motion in tool + // space towards the pose implied by the joint configuration. + ASSERT_TRUE(executor_->moveL(urcl::Q{ -1.572, -1.686, 1.707, -0.833, 0.782, 0.479 }, 1.5, 1.5, 2.0)); +} + +TEST_F(InstructionExecutorTest, movep_accepts_joint_via_motion_target) +{ + // 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_->moveP(urcl::Q{ -1.57, -1.83, 1.707, -0.833, 0.782, 0.479 }, 1.0, 1.0)); +} + +TEST_F(InstructionExecutorTest, movec_accepts_mixed_motion_targets) +{ + // 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_->moveP({ -0.209, 0.492, 0.5522, 0.928, -1.134, -2.168 }, 1.2, 0.25, 0.025)); + + // via as Pose, target as Q + ASSERT_TRUE(executor_->moveC(urcl::MotionTarget{ urcl::Pose{ -0.209, 0.487, 0.671, 1.026, -0.891, -2.337 } }, + urcl::MotionTarget{ urcl::Q{ -1.572, -1.686, 1.707, -0.833, 0.782, 0.479 } }, 0.1, 0.25, + 0.025, 0)); +} + +TEST_F(InstructionExecutorTest, optimovej_accepts_pose_via_motion_target) +{ + 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(urcl::Pose{ -0.203, 0.363, 0.559, 0.68, -1.083, -2.076 }, 0.4, 0.7, 0.1)); +} + +TEST_F(InstructionExecutorTest, optimovel_accepts_joint_via_motion_target) +{ + 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(urcl::Q{ -1.572, -1.686, 1.707, -0.833, 0.782, 0.479 }, 0.4, 0.7, 0.1)); +} + +TEST_F(InstructionExecutorTest, mixed_sequence_with_motion_target_primitives_succeeds) +{ + std::vector> motion_sequence{ + std::make_shared(urcl::vector6d_t{ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 }, 0.1, + std::chrono::seconds(3)), + // MoveL using joint configuration -> MOVEL_JOINT + std::make_shared( + urcl::MotionTarget{ urcl::Q{ -1.572, -1.686, 1.707, -0.833, 0.782, 0.479 } }, 0.1, std::chrono::seconds(2)), + // MoveJ using a pose -> MOVEJ_POSE + std::make_shared( + urcl::MotionTarget{ urcl::Pose{ -0.203, 0.263, 0.559, 0.68, -1.083, -2.076 } }, 0.1, std::chrono::seconds(2)), + }; + ASSERT_TRUE(executor_->executeMotion(motion_sequence)); +} + TEST_F(InstructionExecutorTest, no_new_trajectory_doesnt_stop_execution) { // move to a feasible starting pose diff --git a/tests/test_trajectory_point_interface.cpp b/tests/test_trajectory_point_interface.cpp index 1084ce4c4..7672b58d6 100644 --- a/tests/test_trajectory_point_interface.cpp +++ b/tests/test_trajectory_point_interface.cpp @@ -214,71 +214,113 @@ class TrajectoryPointInterfaceTest : public ::testing::Test return spl; } + static double toDouble(int32_t raw) + { + return static_cast(raw) / control::TrajectoryPointInterface::MULT_JOINTSTATE; + } + + static vector6d_t toVector(const vector6int32_t& raw) + { + return vector6d_t{ toDouble(raw[0]), toDouble(raw[1]), toDouble(raw[2]), + toDouble(raw[3]), toDouble(raw[4]), toDouble(raw[5]) }; + } + + static urcl::Pose toPose(const vector6int32_t& raw) + { + return urcl::Pose{ toDouble(raw[0]), toDouble(raw[1]), toDouble(raw[2]), + toDouble(raw[3]), toDouble(raw[4]), toDouble(raw[5]) }; + } + std::shared_ptr getMotionPrimitive() { TrajData spl = getData(); + const double blend_radius = toDouble(spl.blend_radius_or_spline_type); + const double acceleration = toDouble(spl.acc[0]); + const double velocity = toDouble(spl.vel[0]); + const auto duration = std::chrono::microseconds(spl.goal_time); + if (spl.motion_type == static_cast(control::MotionType::MOVEJ)) { - return std::make_shared( - vector6d_t{ - (double)spl.pos[0] / control::TrajectoryPointInterface::MULT_JOINTSTATE, - (double)spl.pos[1] / control::TrajectoryPointInterface::MULT_JOINTSTATE, - (double)spl.pos[2] / control::TrajectoryPointInterface::MULT_JOINTSTATE, - (double)spl.pos[3] / control::TrajectoryPointInterface::MULT_JOINTSTATE, - (double)spl.pos[4] / control::TrajectoryPointInterface::MULT_JOINTSTATE, - (double)spl.pos[5] / control::TrajectoryPointInterface::MULT_JOINTSTATE, - }, - (double)spl.blend_radius_or_spline_type / control::TrajectoryPointInterface::MULT_JOINTSTATE, - std::chrono::microseconds(spl.goal_time), - (double)spl.acc[0] / control::TrajectoryPointInterface::MULT_JOINTSTATE, - (double)spl.vel[0] / control::TrajectoryPointInterface::MULT_JOINTSTATE); + return std::make_shared(toVector(spl.pos), blend_radius, duration, acceleration, + velocity); + } + else if (spl.motion_type == static_cast(control::MotionType::MOVEJ_POSE)) + { + const auto target = spl.pos; + return std::make_shared(urcl::MotionTarget{ toPose(target) }, blend_radius, duration, + acceleration, velocity); } else if (spl.motion_type == static_cast(control::MotionType::MOVEL)) { + return std::make_shared(toPose(spl.pos), blend_radius, duration, acceleration, + velocity); + } + else if (spl.motion_type == static_cast(control::MotionType::MOVEL_JOINT)) + { + const auto values = toVector(spl.pos); return std::make_shared( - urcl::Pose{ ((double)spl.pos[0]) / control::TrajectoryPointInterface::MULT_JOINTSTATE, - ((double)spl.pos[1]) / control::TrajectoryPointInterface::MULT_JOINTSTATE, - ((double)spl.pos[2]) / control::TrajectoryPointInterface::MULT_JOINTSTATE, - ((double)spl.pos[3]) / control::TrajectoryPointInterface::MULT_JOINTSTATE, - ((double)spl.pos[4]) / control::TrajectoryPointInterface::MULT_JOINTSTATE, - ((double)spl.pos[5]) / control::TrajectoryPointInterface::MULT_JOINTSTATE }, - (double)spl.blend_radius_or_spline_type / control::TrajectoryPointInterface::MULT_JOINTSTATE, - std::chrono::microseconds(spl.goal_time), - (double)spl.acc[0] / control::TrajectoryPointInterface::MULT_JOINTSTATE, - (double)spl.vel[0] / control::TrajectoryPointInterface::MULT_JOINTSTATE); + urcl::MotionTarget{ urcl::Q{ values[0], values[1], values[2], values[3], values[4], values[5] } }, + blend_radius, duration, acceleration, velocity); } else if (spl.motion_type == static_cast(control::MotionType::MOVEP)) { + return std::make_shared(toPose(spl.pos), blend_radius, acceleration, velocity); + } + else if (spl.motion_type == static_cast(control::MotionType::MOVEP_JOINT)) + { + const auto values = toVector(spl.pos); return std::make_shared( - urcl::Pose{ ((double)spl.pos[0]) / control::TrajectoryPointInterface::MULT_JOINTSTATE, - ((double)spl.pos[1]) / control::TrajectoryPointInterface::MULT_JOINTSTATE, - ((double)spl.pos[2]) / control::TrajectoryPointInterface::MULT_JOINTSTATE, - ((double)spl.pos[3]) / control::TrajectoryPointInterface::MULT_JOINTSTATE, - ((double)spl.pos[4]) / control::TrajectoryPointInterface::MULT_JOINTSTATE, - ((double)spl.pos[5]) / control::TrajectoryPointInterface::MULT_JOINTSTATE }, - (double)spl.blend_radius_or_spline_type / control::TrajectoryPointInterface::MULT_JOINTSTATE, - (double)spl.acc[0] / control::TrajectoryPointInterface::MULT_JOINTSTATE, - (double)spl.vel[0] / control::TrajectoryPointInterface::MULT_JOINTSTATE); + urcl::MotionTarget{ urcl::Q{ values[0], values[1], values[2], values[3], values[4], values[5] } }, + blend_radius, acceleration, velocity); + } + else if (spl.motion_type == static_cast(control::MotionType::MOVEC) || + spl.motion_type == static_cast(control::MotionType::MOVEC_JOINT) || + spl.motion_type == static_cast(control::MotionType::MOVEC_POSE_JOINT) || + spl.motion_type == static_cast(control::MotionType::MOVEC_JOINT_POSE)) + { + // For movec the third block holds [velocity, acceleration, mode, 0, 0, 0] + const double movec_velocity = toDouble(spl.acc[0]); + const double movec_acceleration = toDouble(spl.acc[1]); + const int32_t mode = static_cast( + round(static_cast(spl.acc[2]) / control::TrajectoryPointInterface::MULT_JOINTSTATE)); + + auto make_target = [&](bool is_pose, const vector6int32_t& raw) -> urcl::MotionTarget { + if (is_pose) + { + return urcl::MotionTarget{ toPose(raw) }; + } + const auto values = toVector(raw); + return urcl::MotionTarget{ urcl::Q{ values[0], values[1], values[2], values[3], values[4], values[5] } }; + }; + + bool target_is_pose = spl.motion_type == static_cast(control::MotionType::MOVEC) || + spl.motion_type == static_cast(control::MotionType::MOVEC_POSE_JOINT); + bool via_is_pose = spl.motion_type == static_cast(control::MotionType::MOVEC) || + spl.motion_type == static_cast(control::MotionType::MOVEC_JOINT_POSE); + + return std::make_shared(make_target(via_is_pose, spl.vel), + make_target(target_is_pose, spl.pos), blend_radius, + movec_acceleration, movec_velocity, mode); + } + else if (spl.motion_type == static_cast(control::MotionType::OPTIMOVEJ)) + { + return std::make_shared(toVector(spl.pos), blend_radius, acceleration, velocity); + } + else if (spl.motion_type == static_cast(control::MotionType::OPTIMOVEJ_POSE)) + { + return std::make_shared(urcl::MotionTarget{ toPose(spl.pos) }, blend_radius, + acceleration, velocity); + } + else if (spl.motion_type == static_cast(control::MotionType::OPTIMOVEL)) + { + return std::make_shared(toPose(spl.pos), blend_radius, acceleration, velocity); } - else if (spl.motion_type == static_cast(control::MotionType::MOVEC)) + else if (spl.motion_type == static_cast(control::MotionType::OPTIMOVEL_JOINT)) { - return std::make_shared( - urcl::Pose{ ((double)spl.vel[0]) / control::TrajectoryPointInterface::MULT_JOINTSTATE, - ((double)spl.vel[1]) / control::TrajectoryPointInterface::MULT_JOINTSTATE, - ((double)spl.vel[2]) / control::TrajectoryPointInterface::MULT_JOINTSTATE, - ((double)spl.vel[3]) / control::TrajectoryPointInterface::MULT_JOINTSTATE, - ((double)spl.vel[4]) / control::TrajectoryPointInterface::MULT_JOINTSTATE, - ((double)spl.vel[5]) / control::TrajectoryPointInterface::MULT_JOINTSTATE }, - urcl::Pose{ ((double)spl.pos[0]) / control::TrajectoryPointInterface::MULT_JOINTSTATE, - ((double)spl.pos[1]) / control::TrajectoryPointInterface::MULT_JOINTSTATE, - ((double)spl.pos[2]) / control::TrajectoryPointInterface::MULT_JOINTSTATE, - ((double)spl.pos[3]) / control::TrajectoryPointInterface::MULT_JOINTSTATE, - ((double)spl.pos[4]) / control::TrajectoryPointInterface::MULT_JOINTSTATE, - ((double)spl.pos[5]) / control::TrajectoryPointInterface::MULT_JOINTSTATE }, - (double)spl.blend_radius_or_spline_type / control::TrajectoryPointInterface::MULT_JOINTSTATE, - (double)spl.acc[1] / control::TrajectoryPointInterface::MULT_JOINTSTATE, - (double)spl.acc[0] / control::TrajectoryPointInterface::MULT_JOINTSTATE, - static_cast(round((double)spl.acc[2] / control::TrajectoryPointInterface::MULT_JOINTSTATE))); + const auto values = toVector(spl.pos); + return std::make_shared( + urcl::MotionTarget{ urcl::Q{ values[0], values[1], values[2], values[3], values[4], values[5] } }, + blend_radius, acceleration, velocity); } else { @@ -682,6 +724,176 @@ TEST_F(TrajectoryPointInterfaceTest, send_movec) EXPECT_EQ(std::static_pointer_cast(received_primitive)->mode, mode); } +TEST_F(TrajectoryPointInterfaceTest, send_movej_pose) +{ + urcl::Pose send_pose = { 0.1, -0.2, 0.3, 0.4, -0.5, 0.6 }; + double blend_radius = 0.25; + double velocity = 0.6; + double acceleration = 0.7; + auto duration = std::chrono::milliseconds(500); + auto primitive = std::make_shared(urcl::MotionTarget{ send_pose }, blend_radius, duration, + acceleration, velocity); + + traj_point_interface_->writeMotionPrimitive(primitive); + auto received_primitive = client_->getMotionPrimitive(); + EXPECT_EQ(received_primitive->type, control::MotionType::MOVEJ_POSE); + EXPECT_EQ(std::static_pointer_cast(received_primitive)->target_pose, send_pose); + EXPECT_EQ(received_primitive->blend_radius, blend_radius); + EXPECT_EQ(received_primitive->velocity, velocity); + EXPECT_EQ(received_primitive->acceleration, acceleration); + EXPECT_EQ(received_primitive->duration, duration); +} + +TEST_F(TrajectoryPointInterfaceTest, send_movel_joint) +{ + urcl::Q send_joints{ 1.2, 3.1, 2.2, -3.4, -1.1, -1.2 }; + double blend_radius = 0.5; + double velocity = 0.6; + double acceleration = 0.7; + auto duration = std::chrono::milliseconds(434); + auto primitive = std::make_shared(urcl::MotionTarget{ send_joints }, blend_radius, duration, + acceleration, velocity); + + traj_point_interface_->writeMotionPrimitive(primitive); + auto received_primitive = client_->getMotionPrimitive(); + EXPECT_EQ(received_primitive->type, control::MotionType::MOVEL_JOINT); + EXPECT_EQ(std::static_pointer_cast(received_primitive)->target_joint_configuration, + send_joints.values); + EXPECT_EQ(received_primitive->blend_radius, blend_radius); + EXPECT_EQ(received_primitive->velocity, velocity); + EXPECT_EQ(received_primitive->acceleration, acceleration); + EXPECT_EQ(received_primitive->duration, duration); +} + +TEST_F(TrajectoryPointInterfaceTest, send_movep_joint) +{ + urcl::Q send_joints{ 1.2, 3.1, 2.2, -3.4, -1.1, -1.2 }; + double blend_radius = 0.5; + double velocity = 0.6; + double acceleration = 0.7; + auto primitive = std::make_shared(urcl::MotionTarget{ send_joints }, blend_radius, + acceleration, velocity); + + traj_point_interface_->writeMotionPrimitive(primitive); + auto received_primitive = client_->getMotionPrimitive(); + EXPECT_EQ(received_primitive->type, control::MotionType::MOVEP_JOINT); + EXPECT_EQ(std::static_pointer_cast(received_primitive)->target_joint_configuration, + send_joints.values); + EXPECT_EQ(received_primitive->blend_radius, blend_radius); + EXPECT_EQ(received_primitive->velocity, velocity); + EXPECT_EQ(received_primitive->acceleration, acceleration); +} + +TEST_F(TrajectoryPointInterfaceTest, send_movec_joint_joint) +{ + urcl::Q send_via{ 0.5, 0.6, 0.7, 0.8, 0.9, 1.0 }; + urcl::Q send_target{ 1.2, 3.1, 2.2, -3.4, -1.1, -1.2 }; + double blend_radius = 0.5; + double acceleration = 0.7; + double velocity = 0.7; + int32_t mode = 1; + auto primitive = std::make_shared( + urcl::MotionTarget{ send_via }, urcl::MotionTarget{ send_target }, blend_radius, acceleration, velocity, mode); + + traj_point_interface_->writeMotionPrimitive(primitive); + auto received_primitive = client_->getMotionPrimitive(); + EXPECT_EQ(received_primitive->type, control::MotionType::MOVEC_JOINT); + auto movec = std::static_pointer_cast(received_primitive); + EXPECT_EQ(movec->target_joint_configuration, send_target.values); + EXPECT_EQ(movec->via_point_joint_configuration, send_via.values); + EXPECT_EQ(received_primitive->blend_radius, blend_radius); + EXPECT_EQ(received_primitive->acceleration, acceleration); + EXPECT_EQ(received_primitive->velocity, velocity); + EXPECT_EQ(movec->mode, mode); +} + +TEST_F(TrajectoryPointInterfaceTest, send_movec_pose_joint) +{ + // via is a Pose, target is a Q + urcl::Pose send_via{ 0.1, -0.2, 0.3, 0.4, -0.5, 0.6 }; + urcl::Q send_target{ 1.2, 3.1, 2.2, -3.4, -1.1, -1.2 }; + double blend_radius = 0.25; + double acceleration = 0.7; + double velocity = 0.5; + int32_t mode = 0; + auto primitive = std::make_shared( + urcl::MotionTarget{ send_via }, urcl::MotionTarget{ send_target }, blend_radius, acceleration, velocity, mode); + + traj_point_interface_->writeMotionPrimitive(primitive); + auto received_primitive = client_->getMotionPrimitive(); + // The via is a pose, the target is a joint configuration -> MOVEC_JOINT_POSE + EXPECT_EQ(received_primitive->type, control::MotionType::MOVEC_JOINT_POSE); + auto movec = std::static_pointer_cast(received_primitive); + EXPECT_EQ(movec->via_point_pose, send_via); + EXPECT_EQ(movec->target_joint_configuration, send_target.values); + EXPECT_EQ(received_primitive->blend_radius, blend_radius); + EXPECT_EQ(received_primitive->acceleration, acceleration); + EXPECT_EQ(received_primitive->velocity, velocity); + EXPECT_EQ(movec->mode, mode); +} + +TEST_F(TrajectoryPointInterfaceTest, send_movec_joint_pose) +{ + // via is a Q, target is a Pose + urcl::Q send_via{ 0.5, 0.6, 0.7, 0.8, 0.9, 1.0 }; + urcl::Pose send_target{ 1.2, 3.1, 2.2, -3.4, -1.1, -1.2 }; + double blend_radius = 0.25; + double acceleration = 0.7; + double velocity = 0.5; + int32_t mode = 0; + auto primitive = std::make_shared( + urcl::MotionTarget{ send_via }, urcl::MotionTarget{ send_target }, blend_radius, acceleration, velocity, mode); + + traj_point_interface_->writeMotionPrimitive(primitive); + auto received_primitive = client_->getMotionPrimitive(); + // via is a joint configuration, target is a pose -> MOVEC_POSE_JOINT + EXPECT_EQ(received_primitive->type, control::MotionType::MOVEC_POSE_JOINT); + auto movec = std::static_pointer_cast(received_primitive); + EXPECT_EQ(movec->via_point_joint_configuration, send_via.values); + EXPECT_EQ(movec->target_pose, send_target); + EXPECT_EQ(received_primitive->blend_radius, blend_radius); + EXPECT_EQ(received_primitive->acceleration, acceleration); + EXPECT_EQ(received_primitive->velocity, velocity); + EXPECT_EQ(movec->mode, mode); +} + +TEST_F(TrajectoryPointInterfaceTest, send_optimovej_pose) +{ + urcl::Pose send_pose{ 0.1, -0.2, 0.3, 0.4, -0.5, 0.6 }; + double blend_radius = 0.1; + double acceleration_fraction = 0.4; + double velocity_fraction = 0.6; + auto primitive = std::make_shared(urcl::MotionTarget{ send_pose }, blend_radius, + acceleration_fraction, velocity_fraction); + + traj_point_interface_->writeMotionPrimitive(primitive); + auto received_primitive = client_->getMotionPrimitive(); + EXPECT_EQ(received_primitive->type, control::MotionType::OPTIMOVEJ_POSE); + EXPECT_EQ(std::static_pointer_cast(received_primitive)->target_pose, send_pose); + EXPECT_EQ(received_primitive->blend_radius, blend_radius); + EXPECT_EQ(received_primitive->velocity, velocity_fraction); + EXPECT_EQ(received_primitive->acceleration, acceleration_fraction); +} + +TEST_F(TrajectoryPointInterfaceTest, send_optimovel_joint) +{ + urcl::Q send_joints{ 1.2, 3.1, 2.2, -3.4, -1.1, -1.2 }; + double blend_radius = 0.1; + double acceleration_fraction = 0.4; + double velocity_fraction = 0.6; + auto primitive = std::make_shared(urcl::MotionTarget{ send_joints }, blend_radius, + acceleration_fraction, velocity_fraction); + + traj_point_interface_->writeMotionPrimitive(primitive); + auto received_primitive = client_->getMotionPrimitive(); + EXPECT_EQ(received_primitive->type, control::MotionType::OPTIMOVEL_JOINT); + EXPECT_EQ(std::static_pointer_cast(received_primitive)->target_joint_configuration, + send_joints.values); + EXPECT_EQ(received_primitive->blend_radius, blend_radius); + EXPECT_EQ(received_primitive->velocity, velocity_fraction); + EXPECT_EQ(received_primitive->acceleration, acceleration_fraction); +} + TEST_F(TrajectoryPointInterfaceTest, unsupported_motion_type_throws) { auto primitive = std::make_shared(); From 64d3202104b37893731f1e488018e1e067a9cd7a Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 24 Apr 2026 15:07:04 +0200 Subject: [PATCH 07/33] Self-review --- .../control/motion_primitives.h | 161 ++++++++++-------- include/ur_client_library/types.h | 3 + resources/external_control.urscript | 22 +-- src/control/trajectory_point_interface.cpp | 26 ++- src/ur/instruction_executor.cpp | 19 +-- 5 files changed, 141 insertions(+), 90 deletions(-) diff --git a/include/ur_client_library/control/motion_primitives.h b/include/ur_client_library/control/motion_primitives.h index 99d407622..cc0dfed19 100644 --- a/include/ur_client_library/control/motion_primitives.h +++ b/include/ur_client_library/control/motion_primitives.h @@ -34,7 +34,6 @@ #include #include #include -#include "ur_client_library/exceptions.h" #include namespace urcl @@ -76,6 +75,45 @@ enum class MotionType : uint8_t UNKNOWN = 255 }; +inline std::string motionTypeToString(const MotionType type) +{ + switch (type) + { + case MotionType::MOVEJ: + return "MOVEJ"; + case MotionType::MOVEL: + return "MOVEL"; + case MotionType::MOVEP: + return "MOVEP"; + case MotionType::MOVEC: + return "MOVEC"; + case MotionType::OPTIMOVEJ: + return "OPTIMOVEJ"; + case MotionType::OPTIMOVEL: + return "OPTIMOVEL"; + case MotionType::MOVEJ_POSE: + return "MOVEJ_POSE"; + case MotionType::MOVEL_JOINT: + return "MOVEL_JOINT"; + case MotionType::MOVEP_JOINT: + return "MOVEP_JOINT"; + case MotionType::MOVEC_JOINT: + return "MOVEC_JOINT"; + case MotionType::MOVEC_JOINT_POSE: + return "MOVEC_JOINT_POSE"; + case MotionType::MOVEC_POSE_JOINT: + return "MOVEC_POSE_JOINT"; + case MotionType::OPTIMOVEJ_POSE: + return "OPTIMOVEJ_POSE"; + case MotionType::OPTIMOVEL_JOINT: + return "OPTIMOVEL_JOINT"; + case MotionType::SPLINE: + return "SPLINE"; + default: + return "UNKNOWN"; + } +} + /*! * Spline types */ @@ -144,8 +182,8 @@ struct MoveJPrimitive : public MotionPrimitive target); } - urcl::vector6d_t target_joint_configuration; - urcl::Pose target_pose; + urcl::vector6d_t target_joint_configuration{}; + urcl::Pose target_pose{}; }; struct MoveLPrimitive : public MotionPrimitive @@ -195,8 +233,8 @@ struct MoveLPrimitive : public MotionPrimitive target); } - urcl::Pose target_pose; - urcl::vector6d_t target_joint_configuration; + urcl::Pose target_pose{}; + urcl::vector6d_t target_joint_configuration{}; }; struct MovePPrimitive : public MotionPrimitive @@ -239,8 +277,8 @@ struct MovePPrimitive : public MotionPrimitive this->blend_radius = blend_radius; } - urcl::Pose target_pose; - urcl::vector6d_t target_joint_configuration; + urcl::Pose target_pose{}; + urcl::vector6d_t target_joint_configuration{}; }; struct MoveCPrimitive : public MotionPrimitive @@ -261,73 +299,62 @@ struct MoveCPrimitive : public MotionPrimitive * \brief Construct a MoveC primitive from two \ref urcl::MotionTarget values. * * Every combination of \ref urcl::Pose and \ref urcl::Q for the via point and the target is - * supported and mapped to the corresponding \ref MotionType (``MOVEC``, - * ``MOVEC_JOINT``, ``MOVEC_POSE_JOINT``, or ``MOVEC_JOINT_POSE``). When a joint configuration - * is used for either role, only the corresponding joint member is populated and the pose - * member is set to a default-constructed \ref urcl::Pose. + * supported and mapped to the corresponding \ref MotionType (``MOVEC``, ``MOVEC_JOINT``, + * ``MOVEC_POSE_JOINT``, or ``MOVEC_JOINT_POSE``). The naming convention is + * ``MOVEC__``, i.e. ``MOVEC_POSE_JOINT`` denotes a movec whose target is a pose + * and whose via point is a joint configuration. * - * \throws urcl::UrException if either variant is ever extended with an alternative that is - * not handled here. + * Unhandled variant alternatives are caught at compile time via ``static_assert``. */ MoveCPrimitive(const MotionTarget& via_point, const MotionTarget& target, const double blend_radius = 0, const double acceleration = 1.4, const double velocity = 1.04, const int32_t mode = 0) { - if (std::holds_alternative(via_point)) - { - if (std::holds_alternative(target)) - { - type = MotionType::MOVEC_JOINT; - via_point_pose = urcl::Pose(); - target_pose = urcl::Pose(); - via_point_joint_configuration = std::get(via_point).values; - target_joint_configuration = std::get(target).values; - } - else if (std::holds_alternative(target)) - { - type = MotionType::MOVEC_POSE_JOINT; - via_point_pose = urcl::Pose(); - target_pose = std::get(target); - via_point_joint_configuration = std::get(via_point).values; - } - else - { - throw urcl::UrException("Unhandled motion target type for target point passed to MoveCPrimitive constructor"); - } - } - else if (std::holds_alternative(via_point)) - { - if (std::holds_alternative(target)) - { - type = MotionType::MOVEC_JOINT_POSE; - via_point_pose = std::get(via_point); - target_pose = urcl::Pose(); - target_joint_configuration = std::get(target).values; - } - else if (std::holds_alternative(target)) - { - type = MotionType::MOVEC; - via_point_pose = std::get(via_point); - target_pose = std::get(target); - } - else - { - throw urcl::UrException("Unhandled motion target type for target point passed to MoveCPrimitive constructor"); - } - } - else - { - throw urcl::UrException("Unhandled motion target type for via_point passed to MoveCPrimitive constructor"); - } + std::visit( + [&](const auto& via_variant, const auto& target_variant) { + using ViaT = std::decay_t; + using TargetT = std::decay_t; + static_assert(std::is_same_v || std::is_same_v, "Unhandled MotionTarget " + "alternative for via_point"); + static_assert(std::is_same_v || std::is_same_v, "Unhandled MotionTarget " + "alternative for target"); + + if constexpr (std::is_same_v && std::is_same_v) + { + type = MotionType::MOVEC; + via_point_pose = via_variant; + target_pose = target_variant; + } + else if constexpr (std::is_same_v && std::is_same_v) + { + type = MotionType::MOVEC_JOINT; + via_point_joint_configuration = via_variant.values; + target_joint_configuration = target_variant.values; + } + else if constexpr (std::is_same_v && std::is_same_v) + { + type = MotionType::MOVEC_POSE_JOINT; + via_point_joint_configuration = via_variant.values; + target_pose = target_variant; + } + else if constexpr (std::is_same_v && std::is_same_v) + { + type = MotionType::MOVEC_JOINT_POSE; + via_point_pose = via_variant; + target_joint_configuration = target_variant.values; + } + }, + via_point, target); + this->acceleration = acceleration; this->velocity = velocity; this->blend_radius = blend_radius; this->mode = mode; } - urcl::Pose via_point_pose; - urcl::Pose target_pose; - urcl::vector6d_t via_point_joint_configuration; - urcl::vector6d_t target_joint_configuration; + urcl::Pose via_point_pose{}; + urcl::Pose target_pose{}; + urcl::vector6d_t via_point_joint_configuration{}; + urcl::vector6d_t target_joint_configuration{}; int32_t mode = 0; }; @@ -406,8 +433,8 @@ struct OptimoveJPrimitive : public MotionPrimitive bool validate() const override; - urcl::vector6d_t target_joint_configuration; - Pose target_pose; + urcl::vector6d_t target_joint_configuration{}; + Pose target_pose{}; }; struct OptimoveLPrimitive : public MotionPrimitive @@ -452,8 +479,8 @@ struct OptimoveLPrimitive : public MotionPrimitive bool validate() const override; - urcl::Pose target_pose; - vector6d_t target_joint_configuration; + urcl::Pose target_pose{}; + vector6d_t target_joint_configuration{}; }; } // namespace control } // namespace urcl diff --git a/include/ur_client_library/types.h b/include/ur_client_library/types.h index bcf3f7a0c..8cf71ac57 100644 --- a/include/ur_client_library/types.h +++ b/include/ur_client_library/types.h @@ -50,6 +50,9 @@ struct Q constexpr Q(double q1, double q2, double q3, double q4, double q5, double q6) : values{ q1, q2, q3, q4, q5, q6 } { } + explicit constexpr Q(const vector6d_t& values) : values(values) + { + } vector6d_t values; }; diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 371225d88..8d3939937 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -578,7 +578,7 @@ thread trajectoryThread(): acceleration = raw_point[13] / MULT_jointstate velocity = raw_point[7] / MULT_jointstate movej(p[q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, t = tmptime, r = blend_radius) - + # reset old acceleration spline_qdd = [0, 0, 0, 0, 0, 0] spline_qd = [0, 0, 0, 0, 0, 0] @@ -595,12 +595,12 @@ thread trajectoryThread(): elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEL_JOINT: acceleration = raw_point[13] / MULT_jointstate velocity = raw_point[7] / MULT_jointstate - movel([q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, t = tmptime, r = blend_radius) + movel(q, a = acceleration, v = velocity, t = tmptime, r = blend_radius) # reset old acceleration spline_qdd = [0, 0, 0, 0, 0, 0] spline_qd = [0, 0, 0, 0, 0, 0] - + # MoveP point elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEP: acceleration = raw_point[13] / MULT_jointstate @@ -711,7 +711,7 @@ thread trajectoryThread(): elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEL or raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEL_JOINT: 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 %} if raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEL: @@ -766,8 +766,8 @@ thread clearTrajectoryPointsThread(): limit = 20 {% else %} limit = 10 - {% endif %} - + {% endif %} + while trajectory_points_left > 0: raw_point = socket_read_binary_integer(TRAJECTORY_DATA_DIMENSION + 2, "trajectory_socket", timeout) if raw_point[0] <= 0: @@ -778,7 +778,7 @@ thread clearTrajectoryPointsThread(): trajectory_points_left = trajectory_points_left - 1 reads = reads + 1 if reads >= limit: - sync() + sync() reads = 0 end end @@ -999,17 +999,17 @@ end # NODE_CONTROL_LOOP_BEGINS -# Attempt to establish the three required socket connections, if at least one socket is still disconnected, +# Attempt to establish the three required socket connections, if at least one socket is still disconnected, # a popup is shown and the loop continues until all three flags are True. traj_flag = False com_flag = False rev_flag = False while not (traj_flag and com_flag and rev_flag): - if not traj_flag: + if not traj_flag: traj_flag = connect_socket_with_retry("{{SERVER_IP_REPLACE}}", {{TRAJECTORY_SERVER_PORT_REPLACE}}, "trajectory_socket") end - if traj_flag and not com_flag: + if traj_flag and not com_flag: com_flag = connect_socket_with_retry("{{SERVER_IP_REPLACE}}", {{SCRIPT_COMMAND_SERVER_PORT_REPLACE}}, "script_command_socket") end @@ -1052,7 +1052,7 @@ while control_mode > MODE_STOPPED: if params_mult[0] > 0: # Convert read timeout from milliseconds to seconds - read_timeout = params_mult[1] / 1000.0 + read_timeout = params_mult[1] / 1000.0 if control_mode != params_mult[REVERSE_INTERFACE_DATA_DIMENSION]: # Clear remaining trajectory points diff --git a/src/control/trajectory_point_interface.cpp b/src/control/trajectory_point_interface.cpp index db45c6957..94a8a3c60 100644 --- a/src/control/trajectory_point_interface.cpp +++ b/src/control/trajectory_point_interface.cpp @@ -104,7 +104,8 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptrtype) + + " is not allowed in a MoveJPrimitive."); } second_block.fill(primitive->velocity); third_block.fill(primitive->acceleration); @@ -126,7 +127,8 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptrtype) + + " is not allowed in a MoveLPrimitive."); } second_block.fill(primitive->velocity); third_block.fill(primitive->acceleration); @@ -146,6 +148,11 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptrtarget_joint_configuration; } + else + { + throw InvalidData("Motion type " + motionTypeToString(movep_primitive->type) + + " is not allowed in a MovePPrimitive."); + } second_block.fill(primitive->velocity); third_block.fill(primitive->acceleration); break; @@ -184,6 +191,11 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptrvia_point_pose.z, movec_primitive->via_point_pose.rx, movec_primitive->via_point_pose.ry, movec_primitive->via_point_pose.rz }; } + else + { + throw InvalidData("Motion type " + motionTypeToString(movec_primitive->type) + + " is not allowed in a MoveCPrimitive."); + } third_block = { primitive->velocity, primitive->acceleration, static_cast(movec_primitive->mode), 0, 0, 0 }; @@ -214,6 +226,11 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptrtarget_pose.z, optimovej_primitive->target_pose.rx, optimovej_primitive->target_pose.ry, optimovej_primitive->target_pose.rz }; } + else + { + throw InvalidData("Motion type " + motionTypeToString(optimovej_primitive->type) + + " is not allowed in an OptimoveJPrimitive."); + } second_block.fill(primitive->velocity); third_block.fill(primitive->acceleration); break; @@ -232,6 +249,11 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptrtarget_joint_configuration; } + else + { + throw InvalidData("Motion type " + motionTypeToString(optimovel_primitive->type) + + " is not allowed in an OptimoveLPrimitive."); + } second_block.fill(primitive->velocity); third_block.fill(primitive->acceleration); break; diff --git a/src/ur/instruction_executor.cpp b/src/ur/instruction_executor.cpp index c959000c6..ff6ac2e50 100644 --- a/src/ur/instruction_executor.cpp +++ b/src/ur/instruction_executor.cpp @@ -75,7 +75,7 @@ bool urcl::InstructionExecutor::executeMotion( } catch (const UnsupportedMotionType&) { - URCL_LOG_ERROR("Unsupported motion type %d", static_cast(primitive->type)); + URCL_LOG_ERROR("Unsupported motion type %s", motionTypeToString(primitive->type).c_str()); // The hardware will complain about missing trajectory points and return a failure for // trajectory execution. Hence, we need to step into the running loop below. } @@ -100,8 +100,7 @@ bool urcl::InstructionExecutor::executeMotion( bool urcl::InstructionExecutor::moveJ(const urcl::vector6d_t& target, const double acceleration, const double velocity, const double time, const double blend_radius) { - return executeMotion({ std::make_shared( - target, blend_radius, std::chrono::milliseconds(static_cast(time * 1000)), acceleration, velocity) }); + return moveJ(MotionTarget(target), acceleration, velocity, time, blend_radius); } bool urcl::InstructionExecutor::moveJ(const MotionTarget& target, const double acceleration, const double velocity, @@ -114,9 +113,9 @@ bool urcl::InstructionExecutor::moveJ(const MotionTarget& target, const double a bool urcl::InstructionExecutor::moveL(const urcl::Pose& target, const double acceleration, const double velocity, const double time, const double blend_radius) { - return executeMotion({ std::make_shared( - target, blend_radius, std::chrono::milliseconds(static_cast(time * 1000)), acceleration, velocity) }); + return moveL(MotionTarget(target), acceleration, velocity, time, blend_radius); } + bool urcl::InstructionExecutor::moveL(const MotionTarget& target, const double acceleration, const double velocity, const double time, const double blend_radius) { @@ -127,8 +126,9 @@ bool urcl::InstructionExecutor::moveL(const MotionTarget& target, const double a bool urcl::InstructionExecutor::moveP(const urcl::Pose& target, const double acceleration, const double velocity, const double blend_radius) { - return executeMotion({ std::make_shared(target, blend_radius, acceleration, velocity) }); + return moveP(MotionTarget(target), acceleration, velocity, blend_radius); } + bool urcl::InstructionExecutor::moveP(const MotionTarget& target, const double acceleration, const double velocity, const double blend_radius) { @@ -138,8 +138,7 @@ bool urcl::InstructionExecutor::moveP(const MotionTarget& target, const double a bool urcl::InstructionExecutor::moveC(const urcl::Pose& via, const urcl::Pose& target, const double acceleration, const double velocity, const double blend_radius, const int32_t mode) { - return executeMotion( - { std::make_shared(via, target, blend_radius, acceleration, velocity, mode) }); + return moveC(MotionTarget(via), MotionTarget(target), acceleration, velocity, blend_radius, mode); } bool urcl::InstructionExecutor::moveC(const MotionTarget& via, const MotionTarget& target, const double acceleration, @@ -152,7 +151,7 @@ bool urcl::InstructionExecutor::moveC(const MotionTarget& via, const MotionTarge 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) }); + return optimoveJ(MotionTarget(target), acceleration, velocity, blend_radius); } bool urcl::InstructionExecutor::optimoveJ(const MotionTarget& target, const double acceleration, const double velocity, @@ -164,7 +163,7 @@ bool urcl::InstructionExecutor::optimoveJ(const MotionTarget& target, const doub 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) }); + return optimoveL(MotionTarget(target), acceleration, velocity, blend_radius); } bool urcl::InstructionExecutor::optimoveL(const MotionTarget& target, const double acceleration, const double velocity, From 03f993fbe29fff5534e7317fe6d80c5e53e67d80 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 24 Apr 2026 15:34:02 +0200 Subject: [PATCH 08/33] Fix ordering of movec arguments --- resources/external_control.urscript | 4 ++-- tests/test_trajectory_point_interface.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 8d3939937..4e7fbe3d7 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -638,8 +638,8 @@ thread trajectoryThread(): target = q end - acceleration = raw_point[13] / MULT_jointstate - velocity = raw_point[14] / MULT_jointstate + velocity = raw_point[13] / MULT_jointstate + acceleration = raw_point[14] / MULT_jointstate mode = raw_point[15] / MULT_jointstate movec(via, target, acceleration, velocity, blend_radius, mode) diff --git a/tests/test_trajectory_point_interface.cpp b/tests/test_trajectory_point_interface.cpp index 7672b58d6..ad0eae9fd 100644 --- a/tests/test_trajectory_point_interface.cpp +++ b/tests/test_trajectory_point_interface.cpp @@ -707,7 +707,7 @@ TEST_F(TrajectoryPointInterfaceTest, send_movec) urcl::Pose send_target = { 1.2, 3.1, 2.2, -3.4, -1.1, -1.2 }; urcl::Pose send_via = { 0.5, 0.6, 0.7, 0.8, 0.9, 1.0 }; double blend_radius = 0.5; - double acceleration = 0.7; + double acceleration = 0.4; double velocity = 0.7; int32_t mode = 1; auto primitive = @@ -720,7 +720,7 @@ TEST_F(TrajectoryPointInterfaceTest, send_movec) EXPECT_EQ(std::static_pointer_cast(received_primitive)->via_point_pose, send_via); EXPECT_EQ(received_primitive->blend_radius, blend_radius); EXPECT_EQ(received_primitive->acceleration, acceleration); - EXPECT_EQ(received_primitive->acceleration, velocity); + EXPECT_EQ(received_primitive->velocity, velocity); EXPECT_EQ(std::static_pointer_cast(received_primitive)->mode, mode); } From f668a0242ec0bc2a551531388b9610896aeb5a6e Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 27 Apr 2026 07:24:53 +0200 Subject: [PATCH 09/33] Fix conversion --- src/ur/instruction_executor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/ur/instruction_executor.cpp b/src/ur/instruction_executor.cpp index ff6ac2e50..da2d11180 100644 --- a/src/ur/instruction_executor.cpp +++ b/src/ur/instruction_executor.cpp @@ -100,7 +100,7 @@ bool urcl::InstructionExecutor::executeMotion( bool urcl::InstructionExecutor::moveJ(const urcl::vector6d_t& target, const double acceleration, const double velocity, const double time, const double blend_radius) { - return moveJ(MotionTarget(target), acceleration, velocity, time, blend_radius); + return moveJ(Q(target), acceleration, velocity, time, blend_radius); } bool urcl::InstructionExecutor::moveJ(const MotionTarget& target, const double acceleration, const double velocity, @@ -151,7 +151,7 @@ bool urcl::InstructionExecutor::moveC(const MotionTarget& via, const MotionTarge bool urcl::InstructionExecutor::optimoveJ(const urcl::vector6d_t& target, const double acceleration, const double velocity, const double blend_radius) { - return optimoveJ(MotionTarget(target), acceleration, velocity, blend_radius); + return optimoveJ(Q(target), acceleration, velocity, blend_radius); } bool urcl::InstructionExecutor::optimoveJ(const MotionTarget& target, const double acceleration, const double velocity, From 2e26dfe4c001aa25fa4f0e6512c1682e7b397506 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 27 Apr 2026 08:28:19 +0200 Subject: [PATCH 10/33] Removed unreachable code --- include/ur_client_library/exceptions.h | 16 ------------ src/control/trajectory_point_interface.cpp | 30 ---------------------- 2 files changed, 46 deletions(-) diff --git a/include/ur_client_library/exceptions.h b/include/ur_client_library/exceptions.h index 1dda3c78d..9554a80ce 100644 --- a/include/ur_client_library/exceptions.h +++ b/include/ur_client_library/exceptions.h @@ -217,22 +217,6 @@ class UnsupportedMotionType : public UrException } }; -/*! - * \brief Thrown when data received from or passed through the library does not match the - * expected structure or value range. - * - * Examples include motion targets whose variant alternative is not handled by the corresponding - * primitive constructor. - */ -class InvalidData : public UrException -{ -public: - explicit InvalidData() = delete; - explicit InvalidData(const std::string& error_text) : std::runtime_error(error_text) - { - } -}; - class UnknownVariable : public UrException { private: diff --git a/src/control/trajectory_point_interface.cpp b/src/control/trajectory_point_interface.cpp index 94a8a3c60..1c71eb564 100644 --- a/src/control/trajectory_point_interface.cpp +++ b/src/control/trajectory_point_interface.cpp @@ -102,11 +102,6 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptrtarget_pose.z, movej_primitive->target_pose.rx, movej_primitive->target_pose.ry, movej_primitive->target_pose.rz }; } - else - { - throw InvalidData("Motion type " + motionTypeToString(movej_primitive->type) + - " is not allowed in a MoveJPrimitive."); - } second_block.fill(primitive->velocity); third_block.fill(primitive->acceleration); break; @@ -125,11 +120,6 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptrtarget_joint_configuration; } - else - { - throw InvalidData("Motion type " + motionTypeToString(movel_primitive->type) + - " is not allowed in a MoveLPrimitive."); - } second_block.fill(primitive->velocity); third_block.fill(primitive->acceleration); break; @@ -148,11 +138,6 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptrtarget_joint_configuration; } - else - { - throw InvalidData("Motion type " + motionTypeToString(movep_primitive->type) + - " is not allowed in a MovePPrimitive."); - } second_block.fill(primitive->velocity); third_block.fill(primitive->acceleration); break; @@ -191,11 +176,6 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptrvia_point_pose.z, movec_primitive->via_point_pose.rx, movec_primitive->via_point_pose.ry, movec_primitive->via_point_pose.rz }; } - else - { - throw InvalidData("Motion type " + motionTypeToString(movec_primitive->type) + - " is not allowed in a MoveCPrimitive."); - } third_block = { primitive->velocity, primitive->acceleration, static_cast(movec_primitive->mode), 0, 0, 0 }; @@ -226,11 +206,6 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptrtarget_pose.z, optimovej_primitive->target_pose.rx, optimovej_primitive->target_pose.ry, optimovej_primitive->target_pose.rz }; } - else - { - throw InvalidData("Motion type " + motionTypeToString(optimovej_primitive->type) + - " is not allowed in an OptimoveJPrimitive."); - } second_block.fill(primitive->velocity); third_block.fill(primitive->acceleration); break; @@ -249,11 +224,6 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptrtarget_joint_configuration; } - else - { - throw InvalidData("Motion type " + motionTypeToString(optimovel_primitive->type) + - " is not allowed in an OptimoveLPrimitive."); - } second_block.fill(primitive->velocity); third_block.fill(primitive->acceleration); break; From 151a5a3726d19728e376c02cb6e8ea5e14f42324 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 27 Apr 2026 08:28:29 +0200 Subject: [PATCH 11/33] Moved function definition to CPP file --- .../control/motion_primitives.h | 39 +---------------- src/control/motion_primitives.cpp | 42 ++++++++++++++++++- 2 files changed, 42 insertions(+), 39 deletions(-) diff --git a/include/ur_client_library/control/motion_primitives.h b/include/ur_client_library/control/motion_primitives.h index cc0dfed19..8620c8f79 100644 --- a/include/ur_client_library/control/motion_primitives.h +++ b/include/ur_client_library/control/motion_primitives.h @@ -75,44 +75,7 @@ enum class MotionType : uint8_t UNKNOWN = 255 }; -inline std::string motionTypeToString(const MotionType type) -{ - switch (type) - { - case MotionType::MOVEJ: - return "MOVEJ"; - case MotionType::MOVEL: - return "MOVEL"; - case MotionType::MOVEP: - return "MOVEP"; - case MotionType::MOVEC: - return "MOVEC"; - case MotionType::OPTIMOVEJ: - return "OPTIMOVEJ"; - case MotionType::OPTIMOVEL: - return "OPTIMOVEL"; - case MotionType::MOVEJ_POSE: - return "MOVEJ_POSE"; - case MotionType::MOVEL_JOINT: - return "MOVEL_JOINT"; - case MotionType::MOVEP_JOINT: - return "MOVEP_JOINT"; - case MotionType::MOVEC_JOINT: - return "MOVEC_JOINT"; - case MotionType::MOVEC_JOINT_POSE: - return "MOVEC_JOINT_POSE"; - case MotionType::MOVEC_POSE_JOINT: - return "MOVEC_POSE_JOINT"; - case MotionType::OPTIMOVEJ_POSE: - return "OPTIMOVEJ_POSE"; - case MotionType::OPTIMOVEL_JOINT: - return "OPTIMOVEL_JOINT"; - case MotionType::SPLINE: - return "SPLINE"; - default: - return "UNKNOWN"; - } -} +std::string motionTypeToString(const MotionType type); /*! * Spline types diff --git a/src/control/motion_primitives.cpp b/src/control/motion_primitives.cpp index 89cc72171..d8cae58f9 100644 --- a/src/control/motion_primitives.cpp +++ b/src/control/motion_primitives.cpp @@ -97,4 +97,44 @@ bool OptimoveLPrimitive::validate() const } return true; } -} // namespace urcl::control \ No newline at end of file + +std::string motionTypeToString(const MotionType type) +{ + switch (type) + { + case MotionType::MOVEJ: + return "MOVEJ"; + case MotionType::MOVEL: + return "MOVEL"; + case MotionType::MOVEP: + return "MOVEP"; + case MotionType::MOVEC: + return "MOVEC"; + case MotionType::OPTIMOVEJ: + return "OPTIMOVEJ"; + case MotionType::OPTIMOVEL: + return "OPTIMOVEL"; + case MotionType::MOVEJ_POSE: + return "MOVEJ_POSE"; + case MotionType::MOVEL_JOINT: + return "MOVEL_JOINT"; + case MotionType::MOVEP_JOINT: + return "MOVEP_JOINT"; + case MotionType::MOVEC_JOINT: + return "MOVEC_JOINT"; + case MotionType::MOVEC_JOINT_POSE: + return "MOVEC_JOINT_POSE"; + case MotionType::MOVEC_POSE_JOINT: + return "MOVEC_POSE_JOINT"; + case MotionType::OPTIMOVEJ_POSE: + return "OPTIMOVEJ_POSE"; + case MotionType::OPTIMOVEL_JOINT: + return "OPTIMOVEL_JOINT"; + case MotionType::SPLINE: + return "SPLINE"; + default: + return "UNKNOWN"; + } +} + +} // namespace urcl::control From 16d77577b05632d290ea7aa9de3abe60336c8793 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 27 Apr 2026 17:05:30 +0200 Subject: [PATCH 12/33] Refactoring --- .../control/motion_primitives.h | 322 ++++++------------ include/ur_client_library/types.h | 14 +- src/control/motion_primitives.cpp | 257 ++++++++++++++ src/control/trajectory_point_interface.cpp | 137 ++------ tests/test_trajectory_point_interface.cpp | 58 ++-- 5 files changed, 432 insertions(+), 356 deletions(-) diff --git a/include/ur_client_library/control/motion_primitives.h b/include/ur_client_library/control/motion_primitives.h index 8620c8f79..6bc461c86 100644 --- a/include/ur_client_library/control/motion_primitives.h +++ b/include/ur_client_library/control/motion_primitives.h @@ -86,8 +86,15 @@ enum class TrajectorySplineType : int32_t SPLINE_QUINTIC = 2 }; -struct MotionPrimitive +class MotionPrimitive { +public: + MotionPrimitive(const double blend_radius = 0, + const std::chrono::duration duration = std::chrono::milliseconds(0), + const double acceleration = 1.4, const double velocity = 1.04) + : duration(duration), acceleration(acceleration), velocity(velocity), blend_radius(blend_radius) + { + } virtual ~MotionPrimitive() = default; MotionType type = MotionType::UNKNOWN; std::chrono::duration duration; @@ -98,119 +105,85 @@ struct MotionPrimitive virtual bool validate() const; }; -struct MoveJPrimitive : public MotionPrimitive +class MotionPrimitiveWithTarget : public MotionPrimitive { - MoveJPrimitive(const urcl::vector6d_t& target, const double blend_radius = 0, - const std::chrono::duration duration = std::chrono::milliseconds(0), - const double acceleration = 1.4, const double velocity = 1.04) +public: + MotionPrimitiveWithTarget(const MotionTarget& target, const double blend_radius = 0, + const std::chrono::duration duration = std::chrono::milliseconds(0), + const double acceleration = 1.4, const double velocity = 1.04) + : MotionPrimitive(blend_radius, duration, acceleration, velocity) { - type = MotionType::MOVEJ; - target_joint_configuration = target; - this->duration = duration; - this->acceleration = acceleration; - this->velocity = velocity; - this->blend_radius = blend_radius; + setTarget(target); + } + virtual void setTarget(const MotionTarget& target) + { + target_ = std::make_unique(target); } + MotionTarget getTarget() const + { + return *target_; + } + +protected: + std::unique_ptr target_; +}; + +class MoveJPrimitive : public MotionPrimitiveWithTarget +{ +public: + MoveJPrimitive(const urcl::vector6d_t& target, const double blend_radius = 0, + const std::chrono::duration duration = std::chrono::milliseconds(0), + const double acceleration = 1.4, const double velocity = 1.04); + /*! * \brief Construct a MoveJ primitive from a \ref urcl::MotionTarget. * - * If ``target`` holds a \ref urcl::Q, ``type`` is set to \ref MotionType::MOVEJ and - * ``target_joint_configuration`` is populated. If ``target`` holds a \ref urcl::Pose, ``type`` - * is set to \ref MotionType::MOVEJ_POSE and ``target_pose`` is populated instead; the robot - * will internally solve inverse kinematics to reach the pose with a ``movej``. + * If ``target`` holds a \ref urcl::Q, ``type`` is set to \ref MotionType::MOVEJ. If ``target`` + * holds a \ref urcl::Pose, ``type`` is set to \ref MotionType::MOVEJ_POSE; the robot will + * internally solve inverse kinematics to reach the pose with a ``movej``. The stored target is + * accessible via ``getTarget()``. */ MoveJPrimitive(const urcl::MotionTarget& target, const double blend_radius = 0, const std::chrono::duration duration = std::chrono::milliseconds(0), - const double acceleration = 1.4, const double velocity = 1.04) - { - this->duration = duration; - this->acceleration = acceleration; - this->velocity = velocity; - this->blend_radius = blend_radius; - - std::visit( - [&](const auto& target_variant) { - using T = std::decay_t; - if constexpr (std::is_same_v) - { - type = MotionType::MOVEJ_POSE; - target_pose = target_variant; - } - else if constexpr (std::is_same_v) - { - type = MotionType::MOVEJ; - target_joint_configuration = target_variant.values; - } - }, - target); - } + const double acceleration = 1.4, const double velocity = 1.04); + void setTarget(const MotionTarget& target) override; + + [[deprecated("Use getTarget() and setTarget() instead.")]] urcl::vector6d_t target_joint_configuration{}; - urcl::Pose target_pose{}; }; -struct MoveLPrimitive : public MotionPrimitive +class MoveLPrimitive : public MotionPrimitiveWithTarget { +public: MoveLPrimitive(const urcl::Pose& target, const double blend_radius = 0, const std::chrono::duration duration = std::chrono::milliseconds(0), - const double acceleration = 1.4, const double velocity = 1.04) - { - type = MotionType::MOVEL; - target_pose = target; - this->duration = duration; - this->acceleration = acceleration; - this->velocity = velocity; - this->blend_radius = blend_radius; - } + const double acceleration = 1.4, const double velocity = 1.04); + /*! * \brief Construct a MoveL primitive from a \ref urcl::MotionTarget. * * If ``target`` holds a \ref urcl::Pose, ``type`` is set to \ref MotionType::MOVEL. If it - * holds a \ref urcl::Q, ``type`` is set to \ref MotionType::MOVEL_JOINT and the configuration - * is stored in ``target_joint_configuration``. The robot will still execute a tool-space - * linear motion, resolving the joint configuration to its forward kinematics pose on the - * controller. + * holds a \ref urcl::Q, ``type`` is set to \ref MotionType::MOVEL_JOINT. The robot will still + * execute a tool-space linear motion, resolving the joint configuration to its forward + * kinematics pose on the controller. The stored target is accessible via ``getTarget()``. */ MoveLPrimitive(const urcl::MotionTarget& target, const double blend_radius = 0, const std::chrono::duration duration = std::chrono::milliseconds(0), - const double acceleration = 1.4, const double velocity = 1.04) - { - this->duration = duration; - this->acceleration = acceleration; - this->velocity = velocity; - this->blend_radius = blend_radius; - std::visit( - [&](const auto& target_variant) { - using T = std::decay_t; - if constexpr (std::is_same_v) - { - type = MotionType::MOVEL; - target_pose = target_variant; - } - else if constexpr (std::is_same_v) - { - type = MotionType::MOVEL_JOINT; - target_joint_configuration = target_variant.values; - } - }, - target); - } + const double acceleration = 1.4, const double velocity = 1.04); + + void setTarget(const MotionTarget& target) override; + [[deprecated("Use getTarget() and setTarget() instead.")]] urcl::Pose target_pose{}; - urcl::vector6d_t target_joint_configuration{}; }; -struct MovePPrimitive : public MotionPrimitive +class MovePPrimitive : public MotionPrimitiveWithTarget { +public: MovePPrimitive(const urcl::Pose& target, const double blend_radius = 0, const double acceleration = 1.4, - const double velocity = 1.04) - { - type = MotionType::MOVEP; - target_pose = target; - this->acceleration = acceleration; - this->velocity = velocity; - this->blend_radius = blend_radius; - } + const double velocity = 1.04); + /*! * \brief Construct a MoveP primitive from a \ref urcl::MotionTarget. * @@ -218,45 +191,19 @@ struct MovePPrimitive : public MotionPrimitive * \ref MotionType::MOVEP, a \ref urcl::Q selects \ref MotionType::MOVEP_JOINT. */ MovePPrimitive(const MotionTarget& target, const double blend_radius = 0, const double acceleration = 1.4, - const double velocity = 1.04) - { - std::visit( - [&](const auto& target_variant) { - using T = std::decay_t; - if constexpr (std::is_same_v) - { - type = MotionType::MOVEP; - target_pose = target_variant; - } - else if constexpr (std::is_same_v) - { - type = MotionType::MOVEP_JOINT; - target_joint_configuration = target_variant.values; - } - }, - target); - this->acceleration = acceleration; - this->velocity = velocity; - this->blend_radius = blend_radius; - } + const double velocity = 1.04); + + void setTarget(const MotionTarget& target) override; + [[deprecated("Use getTarget() and setTarget() instead.")]] urcl::Pose target_pose{}; - urcl::vector6d_t target_joint_configuration{}; }; -struct MoveCPrimitive : public MotionPrimitive +class MoveCPrimitive : public MotionPrimitiveWithTarget { +public: MoveCPrimitive(const urcl::Pose& via_point, const urcl::Pose& target, const double blend_radius = 0, - const double acceleration = 1.4, const double velocity = 1.04, const int32_t mode = 0) - { - type = MotionType::MOVEC; - via_point_pose = via_point; - target_pose = target; - this->acceleration = acceleration; - this->velocity = velocity; - this->blend_radius = blend_radius; - this->mode = mode; - } + const double acceleration = 1.4, const double velocity = 1.04, const int32_t mode = 0); /*! * \brief Construct a MoveC primitive from two \ref urcl::MotionTarget values. @@ -266,59 +213,32 @@ struct MoveCPrimitive : public MotionPrimitive * ``MOVEC_POSE_JOINT``, or ``MOVEC_JOINT_POSE``). The naming convention is * ``MOVEC__``, i.e. ``MOVEC_POSE_JOINT`` denotes a movec whose target is a pose * and whose via point is a joint configuration. - * - * Unhandled variant alternatives are caught at compile time via ``static_assert``. */ MoveCPrimitive(const MotionTarget& via_point, const MotionTarget& target, const double blend_radius = 0, - const double acceleration = 1.4, const double velocity = 1.04, const int32_t mode = 0) + const double acceleration = 1.4, const double velocity = 1.04, const int32_t mode = 0); + + void setTarget(const MotionTarget& target) override; + + void setVia(const MotionTarget& via_point); + + MotionTarget getVia() const { - std::visit( - [&](const auto& via_variant, const auto& target_variant) { - using ViaT = std::decay_t; - using TargetT = std::decay_t; - static_assert(std::is_same_v || std::is_same_v, "Unhandled MotionTarget " - "alternative for via_point"); - static_assert(std::is_same_v || std::is_same_v, "Unhandled MotionTarget " - "alternative for target"); - - if constexpr (std::is_same_v && std::is_same_v) - { - type = MotionType::MOVEC; - via_point_pose = via_variant; - target_pose = target_variant; - } - else if constexpr (std::is_same_v && std::is_same_v) - { - type = MotionType::MOVEC_JOINT; - via_point_joint_configuration = via_variant.values; - target_joint_configuration = target_variant.values; - } - else if constexpr (std::is_same_v && std::is_same_v) - { - type = MotionType::MOVEC_POSE_JOINT; - via_point_joint_configuration = via_variant.values; - target_pose = target_variant; - } - else if constexpr (std::is_same_v && std::is_same_v) - { - type = MotionType::MOVEC_JOINT_POSE; - via_point_pose = via_variant; - target_joint_configuration = target_variant.values; - } - }, - via_point, target); - - this->acceleration = acceleration; - this->velocity = velocity; - this->blend_radius = blend_radius; - this->mode = mode; + return via_point_; } + [[deprecated("Use getVia() and setVia() instead.")]] urcl::Pose via_point_pose{}; + [[deprecated("Use getTarget() and setTarget() instead.")]] urcl::Pose target_pose{}; - urcl::vector6d_t via_point_joint_configuration{}; - urcl::vector6d_t target_joint_configuration{}; int32_t mode = 0; + +protected: + urcl::MotionTarget via_point_; + +private: + void recomputeType(); + + void refreshLegacyFields(); }; struct SplinePrimitive : public MotionPrimitive @@ -353,17 +273,11 @@ struct SplinePrimitive : public MotionPrimitive std::optional target_accelerations; }; -struct OptimoveJPrimitive : public MotionPrimitive +class OptimoveJPrimitive : public MotionPrimitiveWithTarget { +public: 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; - } + const double acceleration_fraction = 0.3, const double velocity_fraction = 0.3); /*! * \brief Construct an OptimoveJ primitive from a \ref urcl::MotionTarget. @@ -372,45 +286,21 @@ struct OptimoveJPrimitive : public MotionPrimitive * \ref MotionType::OPTIMOVEJ_POSE. */ OptimoveJPrimitive(const MotionTarget& target, const double blend_radius = 0, - const double acceleration_fraction = 0.3, const double velocity_fraction = 0.3) - { - std::visit( - [&](const auto& target_variant) { - using T = std::decay_t; - if constexpr (std::is_same_v) - { - type = MotionType::OPTIMOVEJ_POSE; - target_pose = target_variant; - } - else if constexpr (std::is_same_v) - { - type = MotionType::OPTIMOVEJ; - target_joint_configuration = target_variant.values; - } - }, - target); - this->blend_radius = blend_radius; - this->acceleration = acceleration_fraction; - this->velocity = velocity_fraction; - } + const double acceleration_fraction = 0.3, const double velocity_fraction = 0.3); + + void setTarget(const MotionTarget& target) override; bool validate() const override; + [[deprecated("Use getTarget() and setTarget() instead.")]] urcl::vector6d_t target_joint_configuration{}; - Pose target_pose{}; }; -struct OptimoveLPrimitive : public MotionPrimitive +class OptimoveLPrimitive : public MotionPrimitiveWithTarget { +public: 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; - } + const double velocity_fraction = 0.3); /*! * \brief Construct an OptimoveL primitive from a \ref urcl::MotionTarget. * @@ -418,32 +308,14 @@ struct OptimoveLPrimitive : public MotionPrimitive * \ref MotionType::OPTIMOVEL_JOINT. */ OptimoveLPrimitive(const MotionTarget& target, const double blend_radius = 0, - const double acceleration_fraction = 0.3, const double velocity_fraction = 0.3) - { - std::visit( - [&](const auto& target_variant) { - using T = std::decay_t; - if constexpr (std::is_same_v) - { - type = MotionType::OPTIMOVEL; - target_pose = target_variant; - } - else if constexpr (std::is_same_v) - { - type = MotionType::OPTIMOVEL_JOINT; - target_joint_configuration = target_variant.values; - } - }, - target); - this->blend_radius = blend_radius; - this->acceleration = acceleration_fraction; - this->velocity = velocity_fraction; - } + const double acceleration_fraction = 0.3, const double velocity_fraction = 0.3); + + void setTarget(const MotionTarget& target) override; bool validate() const override; + [[deprecated("Use getTarget() and setTarget() instead.")]] urcl::Pose target_pose{}; - vector6d_t target_joint_configuration{}; }; } // namespace control } // namespace urcl diff --git a/include/ur_client_library/types.h b/include/ur_client_library/types.h index 8cf71ac57..20ace3b0c 100644 --- a/include/ur_client_library/types.h +++ b/include/ur_client_library/types.h @@ -47,14 +47,17 @@ using vector6uint32_t = std::array; */ struct Q { - constexpr Q(double q1, double q2, double q3, double q4, double q5, double q6) : values{ q1, q2, q3, q4, q5, q6 } + Q(const double q1, const double q2, const double q3, const double q4, const double q5, const double q6) { + values = { q1, q2, q3, q4, q5, q6 }; } - explicit constexpr Q(const vector6d_t& values) : values(values) + explicit Q(const vector6d_t& values) { + this->values.resize(6); + std::copy(values.begin(), values.end(), this->values.begin()); } - vector6d_t values; + std::vector values; }; struct Pose @@ -79,6 +82,11 @@ struct Pose } }; +inline bool operator==(const Q& lhs, const vector6d_t& rhs) +{ + return lhs.values.size() == rhs.size() && std::equal(lhs.values.begin(), lhs.values.end(), rhs.begin()); +} + /*! * \brief A tagged union representing either a joint target (\ref Q) or a Cartesian target * (\ref Pose). diff --git a/src/control/motion_primitives.cpp b/src/control/motion_primitives.cpp index d8cae58f9..5d77786d9 100644 --- a/src/control/motion_primitives.cpp +++ b/src/control/motion_primitives.cpp @@ -33,6 +33,8 @@ namespace urcl::control { +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" bool MotionPrimitive::validate() const { @@ -137,4 +139,259 @@ std::string motionTypeToString(const MotionType type) } } +MoveJPrimitive::MoveJPrimitive(const MotionTarget& target, const double blend_radius, + const std::chrono::duration duration, const double acceleration, + const double velocity) + : MotionPrimitiveWithTarget(target, blend_radius, duration, acceleration, velocity) +{ +} + +MoveJPrimitive::MoveJPrimitive(const urcl::vector6d_t& target, const double blend_radius, + const std::chrono::duration duration, const double acceleration, + const double velocity) + : MotionPrimitiveWithTarget(Q(target), blend_radius, duration, acceleration, velocity) +{ +} + +void MoveJPrimitive::setTarget(const MotionTarget& target) +{ + std::visit( + [&](const auto& target_variant) { + using T = std::decay_t; + this->target_ = std::make_unique(target_variant); + if constexpr (std::is_same_v) + { + type = MotionType::MOVEJ_POSE; + + target_joint_configuration.fill(0); + } + else if constexpr (std::is_same_v) + { + type = MotionType::MOVEJ; + std::copy(target_variant.values.begin(), target_variant.values.end(), target_joint_configuration.begin()); + } + }, + target); +} + +MoveLPrimitive::MoveLPrimitive(const urcl::Pose& target, const double blend_radius, + const std::chrono::duration duration, const double acceleration, + const double velocity) + : MotionPrimitiveWithTarget(target, blend_radius, duration, acceleration, velocity) +{ +} + +MoveLPrimitive::MoveLPrimitive(const MotionTarget& target, const double blend_radius, + const std::chrono::duration duration, const double acceleration, + const double velocity) + : MotionPrimitiveWithTarget(target, blend_radius, duration, acceleration, velocity) +{ +} +void MoveLPrimitive::setTarget(const MotionTarget& target) +{ + std::visit( + [&](const auto& target_variant) { + using T = std::decay_t; + target_ = std::make_unique(target_variant); + if constexpr (std::is_same_v) + { + type = MotionType::MOVEL; + target_pose = target_variant; + } + else if constexpr (std::is_same_v) + { + type = MotionType::MOVEL_JOINT; + target_pose = urcl::Pose{}; + } + }, + target); +} + +MovePPrimitive::MovePPrimitive(const urcl::Pose& target, const double blend_radius, const double acceleration, + const double velocity) + : MotionPrimitiveWithTarget(target, blend_radius, std::chrono::milliseconds(0), acceleration, velocity) +{ +} + +MovePPrimitive::MovePPrimitive(const MotionTarget& target, const double blend_radius, const double acceleration, + const double velocity) + : MotionPrimitiveWithTarget(target, blend_radius, std::chrono::milliseconds(0), acceleration, velocity) +{ +} + +void MovePPrimitive::setTarget(const MotionTarget& target) +{ + std::visit( + [&](const auto& target_variant) { + using T = std::decay_t; + target_ = std::make_unique(target_variant); + if constexpr (std::is_same_v) + { + type = MotionType::MOVEP; + target_pose = target_variant; + } + else if constexpr (std::is_same_v) + { + type = MotionType::MOVEP_JOINT; + target_pose = urcl::Pose{}; + } + }, + target); +} + +MoveCPrimitive::MoveCPrimitive(const urcl::Pose& via_point, const urcl::Pose& target, const double blend_radius, + const double acceleration, const double velocity, const int32_t mode) + : MotionPrimitiveWithTarget(target, blend_radius, std::chrono::milliseconds(0), acceleration, velocity) + , mode(mode) + , via_point_(via_point) +{ + setVia(via_point); + setTarget(target); +} + +MoveCPrimitive::MoveCPrimitive(const MotionTarget& via_point, const MotionTarget& target, const double blend_radius, + const double acceleration, const double velocity, const int32_t mode) + : MotionPrimitiveWithTarget(target, blend_radius, std::chrono::milliseconds(0), acceleration, velocity) + , mode(mode) + , via_point_(via_point) +{ + setVia(via_point); + setTarget(target); +} + +void MoveCPrimitive::setTarget(const MotionTarget& target) +{ + target_ = std::make_unique(target); + recomputeType(); + refreshLegacyFields(); +} + +void MoveCPrimitive::setVia(const MotionTarget& via_point) +{ + via_point_ = via_point; + recomputeType(); + refreshLegacyFields(); +} + +void MoveCPrimitive::recomputeType() +{ + const bool target_is_pose = std::holds_alternative(*target_); + const bool via_is_pose = std::holds_alternative(via_point_); + if (target_is_pose && via_is_pose) + { + type = MotionType::MOVEC; + } + else if (!target_is_pose && !via_is_pose) + { + type = MotionType::MOVEC_JOINT; + } + else if (target_is_pose && !via_is_pose) + { + type = MotionType::MOVEC_POSE_JOINT; + } + else + { + type = MotionType::MOVEC_JOINT_POSE; + } +} + +void MoveCPrimitive::refreshLegacyFields() +{ + std::visit( + [&](const auto& target_variant) { + using T = std::decay_t; + if constexpr (std::is_same_v) + { + target_pose = target_variant; + } + else if constexpr (std::is_same_v) + { + target_pose = urcl::Pose{}; + } + }, + *target_); + std::visit( + [&](const auto& via_variant) { + using T = std::decay_t; + if constexpr (std::is_same_v) + { + via_point_pose = via_variant; + } + else if constexpr (std::is_same_v) + { + via_point_pose = urcl::Pose{}; + } + }, + via_point_); +} + +OptimoveJPrimitive::OptimoveJPrimitive(const urcl::vector6d_t& target, const double blend_radius, + const double acceleration_fraction, const double velocity_fraction) + : MotionPrimitiveWithTarget(Q(target), blend_radius, std::chrono::milliseconds(0), acceleration_fraction, + velocity_fraction) +{ +} + +OptimoveJPrimitive::OptimoveJPrimitive(const MotionTarget& target, const double blend_radius, + const double acceleration_fraction, const double velocity_fraction) + : MotionPrimitiveWithTarget(target, blend_radius, std::chrono::milliseconds(0), acceleration_fraction, + velocity_fraction) +{ +} + +void OptimoveJPrimitive::setTarget(const MotionTarget& target) +{ + std::visit( + [&](const auto& target_variant) { + using T = std::decay_t; + this->target_ = std::make_unique(target_variant); + if constexpr (std::is_same_v) + { + type = MotionType::OPTIMOVEJ_POSE; + target_joint_configuration.fill(0); + } + else if constexpr (std::is_same_v) + { + type = MotionType::OPTIMOVEJ; + std::copy(target_variant.values.begin(), target_variant.values.end(), target_joint_configuration.begin()); + } + }, + target); +} + +OptimoveLPrimitive::OptimoveLPrimitive(const urcl::Pose& target, const double blend_radius, + const double acceleration_fraction, const double velocity_fraction) + : MotionPrimitiveWithTarget(target, blend_radius, std::chrono::milliseconds(0), acceleration_fraction, + velocity_fraction) +{ +} + +OptimoveLPrimitive::OptimoveLPrimitive(const MotionTarget& target, const double blend_radius, + const double acceleration_fraction, const double velocity_fraction) + : MotionPrimitiveWithTarget(target, blend_radius, std::chrono::milliseconds(0), acceleration_fraction, + velocity_fraction) +{ +} + +void OptimoveLPrimitive::setTarget(const MotionTarget& target) +{ + std::visit( + [&](const auto& target_variant) { + using T = std::decay_t; + this->target_ = std::make_unique(target_variant); + if constexpr (std::is_same_v) + { + type = MotionType::OPTIMOVEL; + target_pose = target_variant; + } + else if constexpr (std::is_same_v) + { + type = MotionType::OPTIMOVEL_JOINT; + target_pose = urcl::Pose{}; + } + }, + target); +} +#pragma GCC diagnostic pop + } // namespace urcl::control diff --git a/src/control/trajectory_point_interface.cpp b/src/control/trajectory_point_interface.cpp index 1c71eb564..8addd8717 100644 --- a/src/control/trajectory_point_interface.cpp +++ b/src/control/trajectory_point_interface.cpp @@ -39,6 +39,28 @@ namespace urcl { namespace control { +namespace +{ +// Flatten a MotionTarget into a 6-element block, regardless of whether it holds a Q or a Pose. +vector6d_t motionTargetToBlock(const urcl::MotionTarget& target) +{ + return std::visit( + [](const auto& alternative) -> vector6d_t { + using T = std::decay_t; + if constexpr (std::is_same_v) + { + return { alternative.x, alternative.y, alternative.z, alternative.rx, alternative.ry, alternative.rz }; + } + else + { + static_assert(std::is_same_v, "Unhandled MotionTarget alternative"); + return { alternative.values[0], alternative.values[1], alternative.values[2], + alternative.values[3], alternative.values[4], alternative.values[5] }; + } + }, + target); +} +} // namespace std::string trajectoryResultToString(const TrajectoryResult result) { @@ -90,54 +112,17 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptr(primitive); - if (movej_primitive->type == MotionType::MOVEJ) - { - first_block = movej_primitive->target_joint_configuration; - } - else if (movej_primitive->type == MotionType::MOVEJ_POSE) - { - first_block = { movej_primitive->target_pose.x, movej_primitive->target_pose.y, - movej_primitive->target_pose.z, movej_primitive->target_pose.rx, - movej_primitive->target_pose.ry, movej_primitive->target_pose.rz }; - } - second_block.fill(primitive->velocity); - third_block.fill(primitive->acceleration); - break; - } case MotionType::MOVEL: case MotionType::MOVEL_JOINT: - { - auto movel_primitive = std::static_pointer_cast(primitive); - if (movel_primitive->type == MotionType::MOVEL) - { - first_block = { movel_primitive->target_pose.x, movel_primitive->target_pose.y, - movel_primitive->target_pose.z, movel_primitive->target_pose.rx, - movel_primitive->target_pose.ry, movel_primitive->target_pose.rz }; - } - else if (movel_primitive->type == MotionType::MOVEL_JOINT) - { - first_block = movel_primitive->target_joint_configuration; - } - second_block.fill(primitive->velocity); - third_block.fill(primitive->acceleration); - break; - } case MotionType::MOVEP: case MotionType::MOVEP_JOINT: + case MotionType::OPTIMOVEJ: + case MotionType::OPTIMOVEJ_POSE: + case MotionType::OPTIMOVEL: + case MotionType::OPTIMOVEL_JOINT: { - auto movep_primitive = std::static_pointer_cast(primitive); - if (movep_primitive->type == MotionType::MOVEP) - { - first_block = { movep_primitive->target_pose.x, movep_primitive->target_pose.y, - movep_primitive->target_pose.z, movep_primitive->target_pose.rx, - movep_primitive->target_pose.ry, movep_primitive->target_pose.rz }; - } - else if (movep_primitive->type == MotionType::MOVEP_JOINT) - { - first_block = movep_primitive->target_joint_configuration; - } + auto with_target = std::static_pointer_cast(primitive); + first_block = motionTargetToBlock(with_target->getTarget()); second_block.fill(primitive->velocity); third_block.fill(primitive->acceleration); break; @@ -148,34 +133,8 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptr(primitive); - if (movec_primitive->type == MotionType::MOVEC) - { - first_block = { movec_primitive->target_pose.x, movec_primitive->target_pose.y, - movec_primitive->target_pose.z, movec_primitive->target_pose.rx, - movec_primitive->target_pose.ry, movec_primitive->target_pose.rz }; - second_block = { movec_primitive->via_point_pose.x, movec_primitive->via_point_pose.y, - movec_primitive->via_point_pose.z, movec_primitive->via_point_pose.rx, - movec_primitive->via_point_pose.ry, movec_primitive->via_point_pose.rz }; - } - else if (movec_primitive->type == MotionType::MOVEC_JOINT) - { - first_block = movec_primitive->target_joint_configuration; - second_block = movec_primitive->via_point_joint_configuration; - } - else if (movec_primitive->type == MotionType::MOVEC_POSE_JOINT) - { - first_block = { movec_primitive->target_pose.x, movec_primitive->target_pose.y, - movec_primitive->target_pose.z, movec_primitive->target_pose.rx, - movec_primitive->target_pose.ry, movec_primitive->target_pose.rz }; - second_block = movec_primitive->via_point_joint_configuration; - } - else if (movec_primitive->type == MotionType::MOVEC_JOINT_POSE) - { - first_block = movec_primitive->target_joint_configuration; - second_block = { movec_primitive->via_point_pose.x, movec_primitive->via_point_pose.y, - movec_primitive->via_point_pose.z, movec_primitive->via_point_pose.rx, - movec_primitive->via_point_pose.ry, movec_primitive->via_point_pose.rz }; - } + first_block = motionTargetToBlock(movec_primitive->getTarget()); + second_block = motionTargetToBlock(movec_primitive->getVia()); third_block = { primitive->velocity, primitive->acceleration, static_cast(movec_primitive->mode), 0, 0, 0 }; @@ -192,42 +151,6 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptr(primitive); - if (optimovej_primitive->type == MotionType::OPTIMOVEJ) - { - first_block = optimovej_primitive->target_joint_configuration; - } - else if (optimovej_primitive->type == MotionType::OPTIMOVEJ_POSE) - { - first_block = { optimovej_primitive->target_pose.x, optimovej_primitive->target_pose.y, - optimovej_primitive->target_pose.z, optimovej_primitive->target_pose.rx, - optimovej_primitive->target_pose.ry, optimovej_primitive->target_pose.rz }; - } - second_block.fill(primitive->velocity); - third_block.fill(primitive->acceleration); - break; - } - case control::MotionType::OPTIMOVEL: - case control::MotionType::OPTIMOVEL_JOINT: - { - auto optimovel_primitive = std::static_pointer_cast(primitive); - if (optimovel_primitive->type == MotionType::OPTIMOVEL) - { - 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 }; - } - else if (optimovel_primitive->type == MotionType::OPTIMOVEL_JOINT) - { - first_block = optimovel_primitive->target_joint_configuration; - } - second_block.fill(primitive->velocity); - third_block.fill(primitive->acceleration); - break; - } default: throw UnsupportedMotionType(); } diff --git a/tests/test_trajectory_point_interface.cpp b/tests/test_trajectory_point_interface.cpp index ad0eae9fd..0b287f702 100644 --- a/tests/test_trajectory_point_interface.cpp +++ b/tests/test_trajectory_point_interface.cpp @@ -657,8 +657,9 @@ TEST_F(TrajectoryPointInterfaceTest, send_movej) traj_point_interface_->writeMotionPrimitive(primitive); auto received_primitive = client_->getMotionPrimitive(); EXPECT_EQ(received_primitive->type, control::MotionType::MOVEJ); - EXPECT_EQ(std::static_pointer_cast(received_primitive)->target_joint_configuration, - send_positions); + EXPECT_EQ( + std::get(std::static_pointer_cast(received_primitive)->getTarget()), + send_positions); EXPECT_EQ(received_primitive->blend_radius, blend_radius); EXPECT_EQ(received_primitive->velocity, velocity); EXPECT_EQ(received_primitive->acceleration, acceleration); @@ -678,7 +679,9 @@ TEST_F(TrajectoryPointInterfaceTest, send_movel) traj_point_interface_->writeMotionPrimitive(primitive); auto received_primitive = client_->getMotionPrimitive(); EXPECT_EQ(received_primitive->type, control::MotionType::MOVEL); - EXPECT_EQ(std::static_pointer_cast(received_primitive)->target_pose, send_positions); + EXPECT_EQ(std::get( + std::static_pointer_cast(received_primitive)->getTarget()), + send_positions); EXPECT_EQ(received_primitive->blend_radius, blend_radius); EXPECT_EQ(received_primitive->velocity, velocity); EXPECT_EQ(received_primitive->acceleration, acceleration); @@ -696,7 +699,9 @@ TEST_F(TrajectoryPointInterfaceTest, send_movep) traj_point_interface_->writeMotionPrimitive(primitive); auto received_primitive = client_->getMotionPrimitive(); EXPECT_EQ(received_primitive->type, control::MotionType::MOVEP); - EXPECT_EQ(std::static_pointer_cast(received_primitive)->target_pose, send_positions); + EXPECT_EQ(std::get( + std::static_pointer_cast(received_primitive)->getTarget()), + send_positions); EXPECT_EQ(received_primitive->blend_radius, blend_radius); EXPECT_EQ(received_primitive->velocity, velocity); EXPECT_EQ(received_primitive->acceleration, acceleration); @@ -716,12 +721,13 @@ TEST_F(TrajectoryPointInterfaceTest, send_movec) traj_point_interface_->writeMotionPrimitive(primitive); auto received_primitive = client_->getMotionPrimitive(); EXPECT_EQ(received_primitive->type, control::MotionType::MOVEC); - EXPECT_EQ(std::static_pointer_cast(received_primitive)->target_pose, send_target); - EXPECT_EQ(std::static_pointer_cast(received_primitive)->via_point_pose, send_via); + auto received_movec = std::static_pointer_cast(received_primitive); + EXPECT_EQ(std::get(received_movec->getTarget()), send_target); + EXPECT_EQ(std::get(received_movec->getVia()), send_via); EXPECT_EQ(received_primitive->blend_radius, blend_radius); EXPECT_EQ(received_primitive->acceleration, acceleration); EXPECT_EQ(received_primitive->velocity, velocity); - EXPECT_EQ(std::static_pointer_cast(received_primitive)->mode, mode); + EXPECT_EQ(received_movec->mode, mode); } TEST_F(TrajectoryPointInterfaceTest, send_movej_pose) @@ -737,7 +743,9 @@ TEST_F(TrajectoryPointInterfaceTest, send_movej_pose) traj_point_interface_->writeMotionPrimitive(primitive); auto received_primitive = client_->getMotionPrimitive(); EXPECT_EQ(received_primitive->type, control::MotionType::MOVEJ_POSE); - EXPECT_EQ(std::static_pointer_cast(received_primitive)->target_pose, send_pose); + EXPECT_EQ(std::get( + std::static_pointer_cast(received_primitive)->getTarget()), + send_pose); EXPECT_EQ(received_primitive->blend_radius, blend_radius); EXPECT_EQ(received_primitive->velocity, velocity); EXPECT_EQ(received_primitive->acceleration, acceleration); @@ -757,8 +765,10 @@ TEST_F(TrajectoryPointInterfaceTest, send_movel_joint) traj_point_interface_->writeMotionPrimitive(primitive); auto received_primitive = client_->getMotionPrimitive(); EXPECT_EQ(received_primitive->type, control::MotionType::MOVEL_JOINT); - EXPECT_EQ(std::static_pointer_cast(received_primitive)->target_joint_configuration, - send_joints.values); + EXPECT_EQ( + std::get(std::static_pointer_cast(received_primitive)->getTarget()) + .values, + send_joints.values); EXPECT_EQ(received_primitive->blend_radius, blend_radius); EXPECT_EQ(received_primitive->velocity, velocity); EXPECT_EQ(received_primitive->acceleration, acceleration); @@ -777,8 +787,10 @@ TEST_F(TrajectoryPointInterfaceTest, send_movep_joint) traj_point_interface_->writeMotionPrimitive(primitive); auto received_primitive = client_->getMotionPrimitive(); EXPECT_EQ(received_primitive->type, control::MotionType::MOVEP_JOINT); - EXPECT_EQ(std::static_pointer_cast(received_primitive)->target_joint_configuration, - send_joints.values); + EXPECT_EQ( + std::get(std::static_pointer_cast(received_primitive)->getTarget()) + .values, + send_joints.values); EXPECT_EQ(received_primitive->blend_radius, blend_radius); EXPECT_EQ(received_primitive->velocity, velocity); EXPECT_EQ(received_primitive->acceleration, acceleration); @@ -799,8 +811,8 @@ TEST_F(TrajectoryPointInterfaceTest, send_movec_joint_joint) auto received_primitive = client_->getMotionPrimitive(); EXPECT_EQ(received_primitive->type, control::MotionType::MOVEC_JOINT); auto movec = std::static_pointer_cast(received_primitive); - EXPECT_EQ(movec->target_joint_configuration, send_target.values); - EXPECT_EQ(movec->via_point_joint_configuration, send_via.values); + EXPECT_EQ(std::get(movec->getTarget()).values, send_target.values); + EXPECT_EQ(std::get(movec->getVia()).values, send_via.values); EXPECT_EQ(received_primitive->blend_radius, blend_radius); EXPECT_EQ(received_primitive->acceleration, acceleration); EXPECT_EQ(received_primitive->velocity, velocity); @@ -824,8 +836,8 @@ TEST_F(TrajectoryPointInterfaceTest, send_movec_pose_joint) // The via is a pose, the target is a joint configuration -> MOVEC_JOINT_POSE EXPECT_EQ(received_primitive->type, control::MotionType::MOVEC_JOINT_POSE); auto movec = std::static_pointer_cast(received_primitive); - EXPECT_EQ(movec->via_point_pose, send_via); - EXPECT_EQ(movec->target_joint_configuration, send_target.values); + EXPECT_EQ(std::get(movec->getVia()), send_via); + EXPECT_EQ(std::get(movec->getTarget()).values, send_target.values); EXPECT_EQ(received_primitive->blend_radius, blend_radius); EXPECT_EQ(received_primitive->acceleration, acceleration); EXPECT_EQ(received_primitive->velocity, velocity); @@ -849,8 +861,8 @@ TEST_F(TrajectoryPointInterfaceTest, send_movec_joint_pose) // via is a joint configuration, target is a pose -> MOVEC_POSE_JOINT EXPECT_EQ(received_primitive->type, control::MotionType::MOVEC_POSE_JOINT); auto movec = std::static_pointer_cast(received_primitive); - EXPECT_EQ(movec->via_point_joint_configuration, send_via.values); - EXPECT_EQ(movec->target_pose, send_target); + EXPECT_EQ(std::get(movec->getVia()).values, send_via.values); + EXPECT_EQ(std::get(movec->getTarget()), send_target); EXPECT_EQ(received_primitive->blend_radius, blend_radius); EXPECT_EQ(received_primitive->acceleration, acceleration); EXPECT_EQ(received_primitive->velocity, velocity); @@ -869,7 +881,9 @@ TEST_F(TrajectoryPointInterfaceTest, send_optimovej_pose) traj_point_interface_->writeMotionPrimitive(primitive); auto received_primitive = client_->getMotionPrimitive(); EXPECT_EQ(received_primitive->type, control::MotionType::OPTIMOVEJ_POSE); - EXPECT_EQ(std::static_pointer_cast(received_primitive)->target_pose, send_pose); + EXPECT_EQ(std::get( + std::static_pointer_cast(received_primitive)->getTarget()), + send_pose); EXPECT_EQ(received_primitive->blend_radius, blend_radius); EXPECT_EQ(received_primitive->velocity, velocity_fraction); EXPECT_EQ(received_primitive->acceleration, acceleration_fraction); @@ -887,8 +901,10 @@ TEST_F(TrajectoryPointInterfaceTest, send_optimovel_joint) traj_point_interface_->writeMotionPrimitive(primitive); auto received_primitive = client_->getMotionPrimitive(); EXPECT_EQ(received_primitive->type, control::MotionType::OPTIMOVEL_JOINT); - EXPECT_EQ(std::static_pointer_cast(received_primitive)->target_joint_configuration, - send_joints.values); + EXPECT_EQ( + std::get(std::static_pointer_cast(received_primitive)->getTarget()) + .values, + send_joints.values); EXPECT_EQ(received_primitive->blend_radius, blend_radius); EXPECT_EQ(received_primitive->velocity, velocity_fraction); EXPECT_EQ(received_primitive->acceleration, acceleration_fraction); From 398d024d511f4c99c9f5527d45fba2eebd554c3f Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 28 Apr 2026 10:44:38 +0200 Subject: [PATCH 13/33] Correct setting target from derived classes --- .../control/motion_primitives.h | 10 +- src/control/motion_primitives.cpp | 101 ++++++++++-------- 2 files changed, 63 insertions(+), 48 deletions(-) diff --git a/include/ur_client_library/control/motion_primitives.h b/include/ur_client_library/control/motion_primitives.h index 6bc461c86..208271bcb 100644 --- a/include/ur_client_library/control/motion_primitives.h +++ b/include/ur_client_library/control/motion_primitives.h @@ -108,17 +108,15 @@ class MotionPrimitive class MotionPrimitiveWithTarget : public MotionPrimitive { public: - MotionPrimitiveWithTarget(const MotionTarget& target, const double blend_radius = 0, + MotionPrimitiveWithTarget(const double blend_radius = 0, const std::chrono::duration duration = std::chrono::milliseconds(0), const double acceleration = 1.4, const double velocity = 1.04) : MotionPrimitive(blend_radius, duration, acceleration, velocity) { - setTarget(target); - } - virtual void setTarget(const MotionTarget& target) - { - target_ = std::make_unique(target); } + + virtual void setTarget(const MotionTarget& target) = 0; + MotionTarget getTarget() const { return *target_; diff --git a/src/control/motion_primitives.cpp b/src/control/motion_primitives.cpp index 5d77786d9..12cc0b857 100644 --- a/src/control/motion_primitives.cpp +++ b/src/control/motion_primitives.cpp @@ -142,15 +142,17 @@ std::string motionTypeToString(const MotionType type) MoveJPrimitive::MoveJPrimitive(const MotionTarget& target, const double blend_radius, const std::chrono::duration duration, const double acceleration, const double velocity) - : MotionPrimitiveWithTarget(target, blend_radius, duration, acceleration, velocity) + : MotionPrimitiveWithTarget(blend_radius, duration, acceleration, velocity) { + setTarget(target); } MoveJPrimitive::MoveJPrimitive(const urcl::vector6d_t& target, const double blend_radius, const std::chrono::duration duration, const double acceleration, const double velocity) - : MotionPrimitiveWithTarget(Q(target), blend_radius, duration, acceleration, velocity) + : MotionPrimitiveWithTarget(blend_radius, duration, acceleration, velocity) { + setTarget(Q(target)); } void MoveJPrimitive::setTarget(const MotionTarget& target) @@ -177,15 +179,17 @@ void MoveJPrimitive::setTarget(const MotionTarget& target) MoveLPrimitive::MoveLPrimitive(const urcl::Pose& target, const double blend_radius, const std::chrono::duration duration, const double acceleration, const double velocity) - : MotionPrimitiveWithTarget(target, blend_radius, duration, acceleration, velocity) + : MotionPrimitiveWithTarget(blend_radius, duration, acceleration, velocity) { + setTarget(target); } MoveLPrimitive::MoveLPrimitive(const MotionTarget& target, const double blend_radius, const std::chrono::duration duration, const double acceleration, const double velocity) - : MotionPrimitiveWithTarget(target, blend_radius, duration, acceleration, velocity) + : MotionPrimitiveWithTarget(blend_radius, duration, acceleration, velocity) { + setTarget(target); } void MoveLPrimitive::setTarget(const MotionTarget& target) { @@ -209,14 +213,16 @@ void MoveLPrimitive::setTarget(const MotionTarget& target) MovePPrimitive::MovePPrimitive(const urcl::Pose& target, const double blend_radius, const double acceleration, const double velocity) - : MotionPrimitiveWithTarget(target, blend_radius, std::chrono::milliseconds(0), acceleration, velocity) + : MotionPrimitiveWithTarget(blend_radius, std::chrono::milliseconds(0), acceleration, velocity) { + setTarget(target); } MovePPrimitive::MovePPrimitive(const MotionTarget& target, const double blend_radius, const double acceleration, const double velocity) - : MotionPrimitiveWithTarget(target, blend_radius, std::chrono::milliseconds(0), acceleration, velocity) + : MotionPrimitiveWithTarget(blend_radius, std::chrono::milliseconds(0), acceleration, velocity) { + setTarget(target); } void MovePPrimitive::setTarget(const MotionTarget& target) @@ -241,7 +247,7 @@ void MovePPrimitive::setTarget(const MotionTarget& target) MoveCPrimitive::MoveCPrimitive(const urcl::Pose& via_point, const urcl::Pose& target, const double blend_radius, const double acceleration, const double velocity, const int32_t mode) - : MotionPrimitiveWithTarget(target, blend_radius, std::chrono::milliseconds(0), acceleration, velocity) + : MotionPrimitiveWithTarget(blend_radius, std::chrono::milliseconds(0), acceleration, velocity) , mode(mode) , via_point_(via_point) { @@ -251,7 +257,7 @@ MoveCPrimitive::MoveCPrimitive(const urcl::Pose& via_point, const urcl::Pose& ta MoveCPrimitive::MoveCPrimitive(const MotionTarget& via_point, const MotionTarget& target, const double blend_radius, const double acceleration, const double velocity, const int32_t mode) - : MotionPrimitiveWithTarget(target, blend_radius, std::chrono::milliseconds(0), acceleration, velocity) + : MotionPrimitiveWithTarget(blend_radius, std::chrono::milliseconds(0), acceleration, velocity) , mode(mode) , via_point_(via_point) { @@ -275,6 +281,11 @@ void MoveCPrimitive::setVia(const MotionTarget& via_point) void MoveCPrimitive::recomputeType() { + if (target_ == nullptr || via_point_.index() == std::variant_npos) + { + type = MotionType::UNKNOWN; + return; + } const bool target_is_pose = std::holds_alternative(*target_); const bool via_is_pose = std::holds_alternative(via_point_); if (target_is_pose && via_is_pose) @@ -297,46 +308,52 @@ void MoveCPrimitive::recomputeType() void MoveCPrimitive::refreshLegacyFields() { - std::visit( - [&](const auto& target_variant) { - using T = std::decay_t; - if constexpr (std::is_same_v) - { - target_pose = target_variant; - } - else if constexpr (std::is_same_v) - { - target_pose = urcl::Pose{}; - } - }, - *target_); - std::visit( - [&](const auto& via_variant) { - using T = std::decay_t; - if constexpr (std::is_same_v) - { - via_point_pose = via_variant; - } - else if constexpr (std::is_same_v) - { - via_point_pose = urcl::Pose{}; - } - }, - via_point_); + if (target_ != nullptr) + { + std::visit( + [&](const auto& target_variant) { + using T = std::decay_t; + if constexpr (std::is_same_v) + { + target_pose = target_variant; + } + else if constexpr (std::is_same_v) + { + target_pose = urcl::Pose{}; + } + }, + *target_); + } + if (via_point_.index() != std::variant_npos) + { + std::visit( + [&](const auto& via_variant) { + using T = std::decay_t; + if constexpr (std::is_same_v) + { + via_point_pose = via_variant; + } + else if constexpr (std::is_same_v) + { + via_point_pose = urcl::Pose{}; + } + }, + via_point_); + } } OptimoveJPrimitive::OptimoveJPrimitive(const urcl::vector6d_t& target, const double blend_radius, const double acceleration_fraction, const double velocity_fraction) - : MotionPrimitiveWithTarget(Q(target), blend_radius, std::chrono::milliseconds(0), acceleration_fraction, - velocity_fraction) + : MotionPrimitiveWithTarget(blend_radius, std::chrono::milliseconds(0), acceleration_fraction, velocity_fraction) { + setTarget(Q(target)); } OptimoveJPrimitive::OptimoveJPrimitive(const MotionTarget& target, const double blend_radius, const double acceleration_fraction, const double velocity_fraction) - : MotionPrimitiveWithTarget(target, blend_radius, std::chrono::milliseconds(0), acceleration_fraction, - velocity_fraction) + : MotionPrimitiveWithTarget(blend_radius, std::chrono::milliseconds(0), acceleration_fraction, velocity_fraction) { + setTarget(target); } void OptimoveJPrimitive::setTarget(const MotionTarget& target) @@ -361,16 +378,16 @@ void OptimoveJPrimitive::setTarget(const MotionTarget& target) OptimoveLPrimitive::OptimoveLPrimitive(const urcl::Pose& target, const double blend_radius, const double acceleration_fraction, const double velocity_fraction) - : MotionPrimitiveWithTarget(target, blend_radius, std::chrono::milliseconds(0), acceleration_fraction, - velocity_fraction) + : MotionPrimitiveWithTarget(blend_radius, std::chrono::milliseconds(0), acceleration_fraction, velocity_fraction) { + setTarget(target); } OptimoveLPrimitive::OptimoveLPrimitive(const MotionTarget& target, const double blend_radius, const double acceleration_fraction, const double velocity_fraction) - : MotionPrimitiveWithTarget(target, blend_radius, std::chrono::milliseconds(0), acceleration_fraction, - velocity_fraction) + : MotionPrimitiveWithTarget(blend_radius, std::chrono::milliseconds(0), acceleration_fraction, velocity_fraction) { + setTarget(target); } void OptimoveLPrimitive::setTarget(const MotionTarget& target) From fc108f4f75418b8a338b8d46e431ca6b01620910 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 28 Apr 2026 10:45:09 +0200 Subject: [PATCH 14/33] make getTarget return an std::optional --- .../control/motion_primitives.h | 9 +++- src/control/trajectory_point_interface.cpp | 4 +- tests/test_trajectory_point_interface.cpp | 46 +++++++++---------- 3 files changed, 32 insertions(+), 27 deletions(-) diff --git a/include/ur_client_library/control/motion_primitives.h b/include/ur_client_library/control/motion_primitives.h index 208271bcb..04d4b4991 100644 --- a/include/ur_client_library/control/motion_primitives.h +++ b/include/ur_client_library/control/motion_primitives.h @@ -117,9 +117,14 @@ class MotionPrimitiveWithTarget : public MotionPrimitive virtual void setTarget(const MotionTarget& target) = 0; - MotionTarget getTarget() const + [[nodiscard]] + std::optional getTarget() const { - return *target_; + if (target_ != nullptr) + { + return *target_; + } + return std::nullopt; } protected: diff --git a/src/control/trajectory_point_interface.cpp b/src/control/trajectory_point_interface.cpp index 8addd8717..864b75aa0 100644 --- a/src/control/trajectory_point_interface.cpp +++ b/src/control/trajectory_point_interface.cpp @@ -122,7 +122,7 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptr(primitive); - first_block = motionTargetToBlock(with_target->getTarget()); + first_block = motionTargetToBlock(with_target->getTarget().value()); second_block.fill(primitive->velocity); third_block.fill(primitive->acceleration); break; @@ -133,7 +133,7 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptr(primitive); - first_block = motionTargetToBlock(movec_primitive->getTarget()); + first_block = motionTargetToBlock(movec_primitive->getTarget().value()); second_block = motionTargetToBlock(movec_primitive->getVia()); third_block = { primitive->velocity, primitive->acceleration, static_cast(movec_primitive->mode), 0, 0, 0 diff --git a/tests/test_trajectory_point_interface.cpp b/tests/test_trajectory_point_interface.cpp index 0b287f702..efe0d5704 100644 --- a/tests/test_trajectory_point_interface.cpp +++ b/tests/test_trajectory_point_interface.cpp @@ -657,9 +657,9 @@ TEST_F(TrajectoryPointInterfaceTest, send_movej) traj_point_interface_->writeMotionPrimitive(primitive); auto received_primitive = client_->getMotionPrimitive(); EXPECT_EQ(received_primitive->type, control::MotionType::MOVEJ); - EXPECT_EQ( - std::get(std::static_pointer_cast(received_primitive)->getTarget()), - send_positions); + EXPECT_EQ(std::get( + std::static_pointer_cast(received_primitive)->getTarget().value()), + send_positions); EXPECT_EQ(received_primitive->blend_radius, blend_radius); EXPECT_EQ(received_primitive->velocity, velocity); EXPECT_EQ(received_primitive->acceleration, acceleration); @@ -680,7 +680,7 @@ TEST_F(TrajectoryPointInterfaceTest, send_movel) auto received_primitive = client_->getMotionPrimitive(); EXPECT_EQ(received_primitive->type, control::MotionType::MOVEL); EXPECT_EQ(std::get( - std::static_pointer_cast(received_primitive)->getTarget()), + std::static_pointer_cast(received_primitive)->getTarget().value()), send_positions); EXPECT_EQ(received_primitive->blend_radius, blend_radius); EXPECT_EQ(received_primitive->velocity, velocity); @@ -700,7 +700,7 @@ TEST_F(TrajectoryPointInterfaceTest, send_movep) auto received_primitive = client_->getMotionPrimitive(); EXPECT_EQ(received_primitive->type, control::MotionType::MOVEP); EXPECT_EQ(std::get( - std::static_pointer_cast(received_primitive)->getTarget()), + std::static_pointer_cast(received_primitive)->getTarget().value()), send_positions); EXPECT_EQ(received_primitive->blend_radius, blend_radius); EXPECT_EQ(received_primitive->velocity, velocity); @@ -722,7 +722,7 @@ TEST_F(TrajectoryPointInterfaceTest, send_movec) auto received_primitive = client_->getMotionPrimitive(); EXPECT_EQ(received_primitive->type, control::MotionType::MOVEC); auto received_movec = std::static_pointer_cast(received_primitive); - EXPECT_EQ(std::get(received_movec->getTarget()), send_target); + EXPECT_EQ(std::get(received_movec->getTarget().value()), send_target); EXPECT_EQ(std::get(received_movec->getVia()), send_via); EXPECT_EQ(received_primitive->blend_radius, blend_radius); EXPECT_EQ(received_primitive->acceleration, acceleration); @@ -744,7 +744,7 @@ TEST_F(TrajectoryPointInterfaceTest, send_movej_pose) auto received_primitive = client_->getMotionPrimitive(); EXPECT_EQ(received_primitive->type, control::MotionType::MOVEJ_POSE); EXPECT_EQ(std::get( - std::static_pointer_cast(received_primitive)->getTarget()), + std::static_pointer_cast(received_primitive)->getTarget().value()), send_pose); EXPECT_EQ(received_primitive->blend_radius, blend_radius); EXPECT_EQ(received_primitive->velocity, velocity); @@ -765,10 +765,10 @@ TEST_F(TrajectoryPointInterfaceTest, send_movel_joint) traj_point_interface_->writeMotionPrimitive(primitive); auto received_primitive = client_->getMotionPrimitive(); EXPECT_EQ(received_primitive->type, control::MotionType::MOVEL_JOINT); - EXPECT_EQ( - std::get(std::static_pointer_cast(received_primitive)->getTarget()) - .values, - send_joints.values); + EXPECT_EQ(std::get( + std::static_pointer_cast(received_primitive)->getTarget().value()) + .values, + send_joints.values); EXPECT_EQ(received_primitive->blend_radius, blend_radius); EXPECT_EQ(received_primitive->velocity, velocity); EXPECT_EQ(received_primitive->acceleration, acceleration); @@ -787,10 +787,10 @@ TEST_F(TrajectoryPointInterfaceTest, send_movep_joint) traj_point_interface_->writeMotionPrimitive(primitive); auto received_primitive = client_->getMotionPrimitive(); EXPECT_EQ(received_primitive->type, control::MotionType::MOVEP_JOINT); - EXPECT_EQ( - std::get(std::static_pointer_cast(received_primitive)->getTarget()) - .values, - send_joints.values); + EXPECT_EQ(std::get( + std::static_pointer_cast(received_primitive)->getTarget().value()) + .values, + send_joints.values); EXPECT_EQ(received_primitive->blend_radius, blend_radius); EXPECT_EQ(received_primitive->velocity, velocity); EXPECT_EQ(received_primitive->acceleration, acceleration); @@ -811,7 +811,7 @@ TEST_F(TrajectoryPointInterfaceTest, send_movec_joint_joint) auto received_primitive = client_->getMotionPrimitive(); EXPECT_EQ(received_primitive->type, control::MotionType::MOVEC_JOINT); auto movec = std::static_pointer_cast(received_primitive); - EXPECT_EQ(std::get(movec->getTarget()).values, send_target.values); + EXPECT_EQ(std::get(movec->getTarget().value()).values, send_target.values); EXPECT_EQ(std::get(movec->getVia()).values, send_via.values); EXPECT_EQ(received_primitive->blend_radius, blend_radius); EXPECT_EQ(received_primitive->acceleration, acceleration); @@ -837,7 +837,7 @@ TEST_F(TrajectoryPointInterfaceTest, send_movec_pose_joint) EXPECT_EQ(received_primitive->type, control::MotionType::MOVEC_JOINT_POSE); auto movec = std::static_pointer_cast(received_primitive); EXPECT_EQ(std::get(movec->getVia()), send_via); - EXPECT_EQ(std::get(movec->getTarget()).values, send_target.values); + EXPECT_EQ(std::get(movec->getTarget().value()).values, send_target.values); EXPECT_EQ(received_primitive->blend_radius, blend_radius); EXPECT_EQ(received_primitive->acceleration, acceleration); EXPECT_EQ(received_primitive->velocity, velocity); @@ -862,7 +862,7 @@ TEST_F(TrajectoryPointInterfaceTest, send_movec_joint_pose) EXPECT_EQ(received_primitive->type, control::MotionType::MOVEC_POSE_JOINT); auto movec = std::static_pointer_cast(received_primitive); EXPECT_EQ(std::get(movec->getVia()).values, send_via.values); - EXPECT_EQ(std::get(movec->getTarget()), send_target); + EXPECT_EQ(std::get(movec->getTarget().value()), send_target); EXPECT_EQ(received_primitive->blend_radius, blend_radius); EXPECT_EQ(received_primitive->acceleration, acceleration); EXPECT_EQ(received_primitive->velocity, velocity); @@ -882,7 +882,7 @@ TEST_F(TrajectoryPointInterfaceTest, send_optimovej_pose) auto received_primitive = client_->getMotionPrimitive(); EXPECT_EQ(received_primitive->type, control::MotionType::OPTIMOVEJ_POSE); EXPECT_EQ(std::get( - std::static_pointer_cast(received_primitive)->getTarget()), + std::static_pointer_cast(received_primitive)->getTarget().value()), send_pose); EXPECT_EQ(received_primitive->blend_radius, blend_radius); EXPECT_EQ(received_primitive->velocity, velocity_fraction); @@ -901,10 +901,10 @@ TEST_F(TrajectoryPointInterfaceTest, send_optimovel_joint) traj_point_interface_->writeMotionPrimitive(primitive); auto received_primitive = client_->getMotionPrimitive(); EXPECT_EQ(received_primitive->type, control::MotionType::OPTIMOVEL_JOINT); - EXPECT_EQ( - std::get(std::static_pointer_cast(received_primitive)->getTarget()) - .values, - send_joints.values); + EXPECT_EQ(std::get( + std::static_pointer_cast(received_primitive)->getTarget().value()) + .values, + send_joints.values); EXPECT_EQ(received_primitive->blend_radius, blend_radius); EXPECT_EQ(received_primitive->velocity, velocity_fraction); EXPECT_EQ(received_primitive->acceleration, acceleration_fraction); From bcbff6432524a647dbd936c83be19e06c1d9e3a7 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 30 Apr 2026 12:50:34 +0200 Subject: [PATCH 15/33] Support q-near --- examples/instruction_executor.cpp | 12 ++++- .../control/motion_primitives.h | 8 +++ include/ur_client_library/types.h | 31 ++++++++++-- resources/external_control.urscript | 50 +++++++++++-------- src/control/motion_primitives.cpp | 17 +++++++ src/control/trajectory_point_interface.cpp | 29 ++++++++++- 6 files changed, 118 insertions(+), 29 deletions(-) diff --git a/examples/instruction_executor.cpp b/examples/instruction_executor.cpp index f37b5de96..e40a45ffb 100644 --- a/examples/instruction_executor.cpp +++ b/examples/instruction_executor.cpp @@ -104,9 +104,17 @@ int main(int argc, char* argv[]) // acceleration / velocity parametrization brace-init style will be interpreted as joint // positions - instruction_executor->moveJ({ -1.57, -1.57, 0, 0, 0, 0 }, 2.0, 2.0); + instruction_executor->moveJ({ -1.742, -1.726, -2.214, -0.773, 1.572, -0.171 }, 2.0, 2.0); - // goal time parametrization -- acceleration and velocity will be ignored + // Passing a pose to moveJ will make it internally solve inverse kinematics. + instruction_executor->moveJ(urcl::Pose{ -0.206, -0.6437, 0.202, 0.0, 3.140, 0.0 }, 2.0, 2.0); + + // To provide a q_near hint for the IK solver a joint configuration near the target can be added to a pose. + urcl::Pose target_pose{ -0.206, -0.6437, 0.202, 0.0, 3.140, 0.0 }; + target_pose.q_near = urcl::Q{ -1.7, -4, 1.5, -1, 1.5, 0 }; + instruction_executor->moveJ(target_pose); + + // goal time parametrization -- acceleration and velocity will be scaled to meed the goal time. instruction_executor->moveJ({ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 }, 0.1, 0.1, goal_time_sec); // moveL calls with brace-init style is interpreted as a pose diff --git a/include/ur_client_library/control/motion_primitives.h b/include/ur_client_library/control/motion_primitives.h index 04d4b4991..aa5d46caa 100644 --- a/include/ur_client_library/control/motion_primitives.h +++ b/include/ur_client_library/control/motion_primitives.h @@ -175,6 +175,8 @@ class MoveLPrimitive : public MotionPrimitiveWithTarget const std::chrono::duration duration = std::chrono::milliseconds(0), const double acceleration = 1.4, const double velocity = 1.04); + ~MoveLPrimitive() override; + void setTarget(const MotionTarget& target) override; [[deprecated("Use getTarget() and setTarget() instead.")]] @@ -196,6 +198,8 @@ class MovePPrimitive : public MotionPrimitiveWithTarget MovePPrimitive(const MotionTarget& target, const double blend_radius = 0, const double acceleration = 1.4, const double velocity = 1.04); + ~MovePPrimitive() override; + void setTarget(const MotionTarget& target) override; [[deprecated("Use getTarget() and setTarget() instead.")]] @@ -220,6 +224,8 @@ class MoveCPrimitive : public MotionPrimitiveWithTarget MoveCPrimitive(const MotionTarget& via_point, const MotionTarget& target, const double blend_radius = 0, const double acceleration = 1.4, const double velocity = 1.04, const int32_t mode = 0); + ~MoveCPrimitive() override; + void setTarget(const MotionTarget& target) override; void setVia(const MotionTarget& via_point); @@ -313,6 +319,8 @@ class OptimoveLPrimitive : public MotionPrimitiveWithTarget OptimoveLPrimitive(const MotionTarget& target, const double blend_radius = 0, const double acceleration_fraction = 0.3, const double velocity_fraction = 0.3); + ~OptimoveLPrimitive() override; + void setTarget(const MotionTarget& target) override; bool validate() const override; diff --git a/include/ur_client_library/types.h b/include/ur_client_library/types.h index 20ace3b0c..c5780febb 100644 --- a/include/ur_client_library/types.h +++ b/include/ur_client_library/types.h @@ -21,9 +21,11 @@ #pragma once #include +#include #include #include #include +#include #include #include "ur_client_library/log.h" @@ -60,13 +62,18 @@ struct Q std::vector values; }; +inline bool operator==(const Q& lhs, const Q& rhs) +{ + return lhs.values.size() == rhs.values.size() && std::equal(lhs.values.begin(), lhs.values.end(), rhs.values.begin()); +} + struct Pose { - Pose() : x(0.0), y(0.0), z(0.0), rx(0.0), ry(0.0), rz(0.0) + Pose() : x(0.0), y(0.0), z(0.0), rx(0.0), ry(0.0), rz(0.0), q_near(std::nullopt) { } Pose(const double x, const double y, const double z, const double rx, const double ry, const double rz) - : x(x), y(y), z(z), rx(rx), ry(ry), rz(rz) + : x(x), y(y), z(z), rx(rx), ry(ry), rz(rz), q_near(std::nullopt) { } double x; @@ -76,9 +83,27 @@ struct Pose double ry; double rz; + /*! + * Optional joint-space hint (six joint positions in radians) passed to the controller for inverse + * kinematics when this pose is used as a Cartesian motion target over the trajectory interface. + */ + std::optional q_near; + bool operator==(const Pose& other) const { - return x == other.x && y == other.y && z == other.z && rx == other.rx && ry == other.ry && rz == other.rz; + if (x != other.x || y != other.y || z != other.z || rx != other.rx || ry != other.ry || rz != other.rz) + { + return false; + } + if (q_near.has_value() != other.q_near.has_value()) + { + return false; + } + if (!q_near.has_value()) + { + return true; + } + return *q_near == *other.q_near; } }; diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 4e7fbe3d7..ba3abf3a5 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -565,19 +565,29 @@ thread trajectoryThread(): blend_radius = 0.0 is_last_point = True end + + second_block = [raw_point[7], raw_point[8], raw_point[9], raw_point[10], raw_point[11], raw_point[12]] / MULT_jointstate + velocity = raw_point[13] / MULT_jointstate + acceleration = raw_point[14] / MULT_jointstate + has_qnear = raw_point[15] / MULT_jointstate + # MoveJ point if raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEJ: - acceleration = raw_point[13] / MULT_jointstate - velocity = raw_point[7] / MULT_jointstate + textmsg("Moving to joint position: ", q) movej(q, a = acceleration, v = velocity, t = tmptime, r = blend_radius) # reset old acceleration spline_qdd = [0, 0, 0, 0, 0, 0] spline_qd = [0, 0, 0, 0, 0, 0] elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEJ_POSE: - acceleration = raw_point[13] / MULT_jointstate - velocity = raw_point[7] / MULT_jointstate - movej(p[q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, t = tmptime, r = blend_radius) + target_pose = p[q[0], q[1], q[2], q[3], q[4], q[5]] + textmsg("Moving to pose: ", target_pose) + target_q = get_inverse_kin(target_pose, get_target_joint_positions()) + if has_qnear: + target_q = get_inverse_kin(target_pose, second_block) + end + movej(target_q, a = acceleration, v = velocity, t = tmptime, r = blend_radius) + # reset old acceleration spline_qdd = [0, 0, 0, 0, 0, 0] @@ -585,16 +595,12 @@ thread trajectoryThread(): # Movel point elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEL: - acceleration = raw_point[13] / MULT_jointstate - velocity = raw_point[7] / MULT_jointstate movel(p[q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, t = tmptime, r = blend_radius) # reset old acceleration spline_qdd = [0, 0, 0, 0, 0, 0] spline_qd = [0, 0, 0, 0, 0, 0] elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEL_JOINT: - acceleration = raw_point[13] / MULT_jointstate - velocity = raw_point[7] / MULT_jointstate movel(q, a = acceleration, v = velocity, t = tmptime, r = blend_radius) # reset old acceleration @@ -603,16 +609,12 @@ thread trajectoryThread(): # MoveP point elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEP: - acceleration = raw_point[13] / MULT_jointstate - velocity = raw_point[7] / MULT_jointstate movep(p[q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, r = blend_radius) # reset old acceleration spline_qdd = [0, 0, 0, 0, 0, 0] spline_qd = [0, 0, 0, 0, 0, 0] elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEP_JOINT: - acceleration = raw_point[13] / MULT_jointstate - velocity = raw_point[7] / MULT_jointstate movep(q, a = acceleration, v = velocity, r = blend_radius) # reset old acceleration @@ -623,7 +625,7 @@ thread trajectoryThread(): raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEC_JOINT or raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEC_POSE_JOINT or raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEC_JOINT_POSE: - local v = [raw_point[7], raw_point[8], raw_point[9], raw_point[10], raw_point[11], raw_point[12]] / MULT_jointstate + local v = second_block if raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEC: via = p[v[0], v[1], v[2], v[3], v[4], v[5]] target = p[q[0], q[1], q[2], q[3], q[4], q[5]] @@ -638,8 +640,6 @@ thread trajectoryThread(): target = q end - velocity = raw_point[13] / MULT_jointstate - acceleration = raw_point[14] / MULT_jointstate mode = raw_point[15] / MULT_jointstate movec(via, target, acceleration, velocity, blend_radius, mode) @@ -673,20 +673,28 @@ thread trajectoryThread(): # OptimoveJ point elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEJ or raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEJ_POSE: - 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 %} if raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEJ: optimovej(q, a = acceleration, v = velocity, r = blend_radius) else: - optimovej(p[q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, r = blend_radius) + if has_qnear: + target_q = get_inverse_kin(p[q[0], q[1], q[2], q[3], q[4], q[5]], second_block) + else: + target_q = get_inverse_kin(p[q[0], q[1], q[2], q[3], q[4], q[5]], get_target_joint_positions()) + end + optimovej(target_q, a = acceleration, v = velocity, r = blend_radius) end {% elif ROBOT_SOFTWARE_VERSION >= v10.8.0 %} if raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEJ: optimovej(q, a = acceleration, v = velocity, r = blend_radius) else: - optimovej(p[q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, r = blend_radius) + if has_qnear: + target_q = get_inverse_kin(p[q[0], q[1], q[2], q[3], q[4], q[5]], second_block) + else: + target_q = get_inverse_kin(p[q[0], q[1], q[2], q[3], q[4], q[5]], get_target_joint_positions()) + end + optimovej(target_q, a = acceleration, v = velocity, r = blend_radius) end {% else %} popup("Optimovej is only supported from software 10.8.0 and upwards.", error=True, blocking=False) @@ -709,8 +717,6 @@ thread trajectoryThread(): # OptimoveL point elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEL or raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEL_JOINT: - 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 %} diff --git a/src/control/motion_primitives.cpp b/src/control/motion_primitives.cpp index 12cc0b857..2a8c857b1 100644 --- a/src/control/motion_primitives.cpp +++ b/src/control/motion_primitives.cpp @@ -191,6 +191,11 @@ MoveLPrimitive::MoveLPrimitive(const MotionTarget& target, const double blend_ra { setTarget(target); } + +MoveLPrimitive::~MoveLPrimitive() +{ +} + void MoveLPrimitive::setTarget(const MotionTarget& target) { std::visit( @@ -225,6 +230,10 @@ MovePPrimitive::MovePPrimitive(const MotionTarget& target, const double blend_ra setTarget(target); } +MovePPrimitive::~MovePPrimitive() +{ +} + void MovePPrimitive::setTarget(const MotionTarget& target) { std::visit( @@ -265,6 +274,10 @@ MoveCPrimitive::MoveCPrimitive(const MotionTarget& via_point, const MotionTarget setTarget(target); } +MoveCPrimitive::~MoveCPrimitive() +{ +} + void MoveCPrimitive::setTarget(const MotionTarget& target) { target_ = std::make_unique(target); @@ -390,6 +403,10 @@ OptimoveLPrimitive::OptimoveLPrimitive(const MotionTarget& target, const double setTarget(target); } +OptimoveLPrimitive::~OptimoveLPrimitive() +{ +} + void OptimoveLPrimitive::setTarget(const MotionTarget& target) { std::visit( diff --git a/src/control/trajectory_point_interface.cpp b/src/control/trajectory_point_interface.cpp index 864b75aa0..efec8b2d2 100644 --- a/src/control/trajectory_point_interface.cpp +++ b/src/control/trajectory_point_interface.cpp @@ -123,8 +123,33 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptr(primitive); first_block = motionTargetToBlock(with_target->getTarget().value()); - second_block.fill(primitive->velocity); - third_block.fill(primitive->acceleration); + bool has_qnear = false; + second_block = std::visit( + [&has_qnear](const auto& alternative) -> vector6d_t { + using T = std::decay_t; + if constexpr (std::is_same_v) + { + if (alternative.q_near.has_value()) + { + has_qnear = true; + return vector6d_t{ alternative.q_near->values[0], alternative.q_near->values[1], + alternative.q_near->values[2], alternative.q_near->values[3], + alternative.q_near->values[4], alternative.q_near->values[5] }; + } + else + { + has_qnear = false; + return vector6d_t{ 0, 0, 0, 0, 0, 0 }; + } + } + else + { + static_assert(std::is_same_v, "Unhandled MotionTarget alternative"); + return { 0, 0, 0, 0, 0, 0 }; + } + }, + with_target->getTarget().value()); + third_block = { primitive->velocity, primitive->acceleration, static_cast(has_qnear), 0, 0, 0 }; break; } case MotionType::MOVEC: From 856c9421c2d43813376a1d99ea9e49619432d52e Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 30 Apr 2026 12:33:59 +0200 Subject: [PATCH 16/33] Update docs --- .../trajectory_point_interface.rst | 30 +++++++++++++------ 1 file changed, 21 insertions(+), 9 deletions(-) diff --git a/doc/architecture/trajectory_point_interface.rst b/doc/architecture/trajectory_point_interface.rst index b22cf8f34..f745ef070 100644 --- a/doc/architecture/trajectory_point_interface.rst +++ b/doc/architecture/trajectory_point_interface.rst @@ -46,19 +46,31 @@ representations in 21 datafields. The data fields have the following meaning: Interpreted as joint positions [rad] or as a Cartesian pose ([m, m, m, rad, rad, rad]) depending on the motion type at index 18 (see below). - 6-11 trajectory point velocities (multiplied by ``MULT_JOINTSTATE``). + 6-11 Depending on the motion type, this represents either - For all MOVEC variants this field is repurposed to carry the via point (same - joint-vs-pose interpretation as the target at indices 0-5, see the motion type at - index 18). + - Q-near (joint configuration closest to the target pose) when passing a pose to MoveJ or + OptimoveJ (``MOVEJ_POSE`` and ``OPTIMOVEJ_POSE``). Used, when the "has Q-near" (see + byte 14) flag is set. - 12-17 trajectory point accelerations (multiplied by ``MULT_JOINTSTATE``). + - For all MOVEC variants this field contains the via point (same + joint-vs-pose interpretation as the target at indices 0-5, see the motion type at + index 18). + - trajectory point velicities (multiplied by ``MULT_JOINTSTATE``) for spline joint types - For all MOVEC variants: + 12-17 Depending on the motion type, this represents either + + - trajectory point accelerations (multiplied by ``MULT_JOINTSTATE``) for spline joint + types. + + - for all other motion types + + - 12: velocity (multiplied by ``MULT_JOINTSTATE``) + - 13: acceleration (multiplied by ``MULT_JOINTSTATE``) + - 14: Depending on motion type: + + - ``MOVEC_*``: mode (multiplied by ``MULT_JOINTSTATE``) + - ``MOVEJ_POSE`` and ``OPTIMOVEJ_POSE``: boolean "has Q-near" (multiplied by ``MULT_JOINTSTATE``) - - 12: velocity (multiplied by ``MULT_JOINTSTATE``) - - 13: acceleration (multiplied by ``MULT_JOINTSTATE``) - - 14: mode (multiplied by ``MULT_JOINTSTATE``) 18 trajectory point type. The base values below use the URScript command's "natural" target type (joints for ``movej`` / ``optimovej``, Cartesian pose for ``movel`` / From 6c5e79f4a1ce681939974100b583894006da468d Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 30 Apr 2026 13:25:07 +0200 Subject: [PATCH 17/33] Update documentation --- doc/architecture/instruction_executor.rst | 3 ++ .../trajectory_point_interface.rst | 39 +++++++++++-------- doc/examples/instruction_executor.rst | 4 ++ 3 files changed, 29 insertions(+), 17 deletions(-) diff --git a/doc/architecture/instruction_executor.rst b/doc/architecture/instruction_executor.rst index 44169945b..1f249ca96 100644 --- a/doc/architecture/instruction_executor.rst +++ b/doc/architecture/instruction_executor.rst @@ -32,6 +32,9 @@ Every motion function comes in two flavours: a Cartesian target with a joint-interpolated motion, or ``moveL(urcl::Q{...})`` to perform a linear tool-space motion towards the pose implied by a joint configuration. +An ``urcl::Pose`` may optionally carry ``q_near`` (``std::optional``), a joint configuration +hint for IK when the pose is sent over the trajectory interface; see :ref:`trajectory_point_interface`. + The Instruction Executor uses the :ref:`trajectory_point_interface` and the :ref:`reverse_interface` for sending motion instructions to the robot. Hence, it requires a :ref:`ur_driver` object. diff --git a/doc/architecture/trajectory_point_interface.rst b/doc/architecture/trajectory_point_interface.rst index f745ef070..2a08a2820 100644 --- a/doc/architecture/trajectory_point_interface.rst +++ b/doc/architecture/trajectory_point_interface.rst @@ -44,18 +44,20 @@ representations in 21 datafields. The data fields have the following meaning: 0-5 trajectory point positions (multiplied by ``MULT_JOINTSTATE``). Interpreted as joint positions [rad] or as a Cartesian pose ([m, m, m, rad, rad, rad]) - depending on the motion type at index 18 (see below). + depending on the motion type at index 20 (see below). 6-11 Depending on the motion type, this represents either - - Q-near (joint configuration closest to the target pose) when passing a pose to MoveJ or - OptimoveJ (``MOVEJ_POSE`` and ``OPTIMOVEJ_POSE``). Used, when the "has Q-near" (see - byte 14) flag is set. + - Joint hint ``q_near`` (six joint positions in radians, ``MULT_JOINTSTATE``-scaled) when + the motion target is a Cartesian ``urcl::Pose`` with ``Pose::q_near`` set on the library + side. The bundled ``external_control.urscript`` applies this hint for ``MOVEJ_POSE`` and + ``OPTIMOVEJ_POSE`` (``get_inverse_kin`` seed); other pose motion types still receive the + block on the wire but ignore it in the default script. - For all MOVEC variants this field contains the via point (same joint-vs-pose interpretation as the target at indices 0-5, see the motion type at - index 18). - - trajectory point velicities (multiplied by ``MULT_JOINTSTATE``) for spline joint types + index 20). + - trajectory point velocities (multiplied by ``MULT_JOINTSTATE``) for spline joint types 12-17 Depending on the motion type, this represents either @@ -68,11 +70,21 @@ representations in 21 datafields. The data fields have the following meaning: - 13: acceleration (multiplied by ``MULT_JOINTSTATE``) - 14: Depending on motion type: - - ``MOVEC_*``: mode (multiplied by ``MULT_JOINTSTATE``) - - ``MOVEJ_POSE`` and ``OPTIMOVEJ_POSE``: boolean "has Q-near" (multiplied by ``MULT_JOINTSTATE``) + - ``MOVEC_*``: arc mode (multiplied by ``MULT_JOINTSTATE``) + - Other non-spline primitives with a Cartesian ``Pose`` target: ``1.0`` or ``0.0`` + (scaled by ``MULT_JOINTSTATE``) indicating whether indices 6–11 carry ``q_near``. + The bundled URScript consumes ``q_near`` for ``MOVEJ_POSE`` and ``OPTIMOVEJ_POSE`` + only; the flag and joint block are still defined this way for all such pose motions. + 18 segment duration (seconds, multiplied by ``MULT_TIME``; integer microseconds on the wire). - 18 trajectory point type. The base values below use the URScript command's "natural" + 19 depending on trajectory point type + + - All MOVE* and OPTIMOVE* variants: point blend radius (in meters, multiplied by + ``MULT_TIME``) + - SPLINE: spline type (1: CUBIC, 2: QUINTIC; raw integer, not ``MULT_TIME``-scaled) + + 20 trajectory point type. The base values below use the URScript command's "natural" target type (joints for ``movej`` / ``optimovej``, Cartesian pose for ``movel`` / ``movep`` / ``movec`` / ``optimovel``). The ``*_POSE`` / ``*_JOINT`` variants indicate that the other target kind is being sent instead. @@ -94,13 +106,6 @@ representations in 21 datafields. The data fields have the following meaning: - 12: OPTIMOVEJ_POSE – ``optimovej`` to a pose target - 13: OPTIMOVEL_JOINT – ``optimovel`` to the pose implied by a joint target - 51: SPLINE - - 19 trajectory point time (in seconds, multiplied by ``MULT_TIME``) - 20 depending on trajectory point type - - - All MOVE* and OPTIMOVE* variants: point blend radius (in meters, multiplied by - ``MULT_TIME``) - - SPLINE: spline type (1: CUBIC, 2: QUINTIC) ===== ===== where @@ -118,7 +123,7 @@ where targets freely through the high-level APIs (see :ref:`instruction_executor` and the ``urcl::MotionTarget`` type). On the wire the positions are always packed as a 6-tuple of ``MULT_JOINTSTATE``-scaled integers; which physical quantity they represent (joint angles or - Cartesian pose components) is determined solely by the motion type field at index 18. The + Cartesian pose components) is determined solely by the motion type field at index 20. The corresponding mapping back to ``movej`` / ``movel`` / ``movep`` / ``movec`` / ``optimovej`` / ``optimovel`` calls with either ``q`` or ``p[...]`` arguments is performed on the robot side by ``resources/external_control.urscript``. diff --git a/doc/examples/instruction_executor.rst b/doc/examples/instruction_executor.rst index 084b9fbed..04bc9b71c 100644 --- a/doc/examples/instruction_executor.rst +++ b/doc/examples/instruction_executor.rst @@ -84,3 +84,7 @@ natural overload, so ``moveJ({ ... })`` continues to be interpreted as joint pos ``moveJ(urcl::Pose{...})`` performs a ``movej`` towards a Cartesian target and ``moveL(urcl::Q{...})`` performs a ``movel`` towards the pose implied by a joint configuration. The same rules apply to ``moveP``, ``moveC``, ``optimoveJ`` and ``optimoveL``. + +For Cartesian targets, ``urcl::Pose`` may optionally set ``q_near`` (a ``urcl::Q``): a joint-space +hint forwarded on the trajectory socket for inverse kinematics. The bundled external-control +URScript uses it for ``movej`` / ``optimovej`` to a pose; see :ref:`trajectory_point_interface`. From 7e047bfce50fc43029b2cb6f9a7f03cdb935b597 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 30 Apr 2026 13:50:21 +0200 Subject: [PATCH 18/33] Update tests --- tests/test_trajectory_point_interface.cpp | 155 ++++++++++++++++------ 1 file changed, 115 insertions(+), 40 deletions(-) diff --git a/tests/test_trajectory_point_interface.cpp b/tests/test_trajectory_point_interface.cpp index efe0d5704..e79efa37c 100644 --- a/tests/test_trajectory_point_interface.cpp +++ b/tests/test_trajectory_point_interface.cpp @@ -35,6 +35,7 @@ #include #include #include +#include #include "ur_client_library/exceptions.h" using namespace urcl; @@ -231,12 +232,26 @@ class TrajectoryPointInterfaceTest : public ::testing::Test toDouble(raw[3]), toDouble(raw[4]), toDouble(raw[5]) }; } + /*! \brief Decode Cartesian pose from block 0; if block 2 index 2 is non-zero, attach ``q_near`` from block 1. */ + static urcl::Pose poseWithOptionalQNear(const vector6int32_t& pose_raw, const vector6int32_t& second_raw, + const vector6int32_t& third_raw) + { + urcl::Pose pose = toPose(pose_raw); + if (toDouble(third_raw[2]) != 0.0) + { + pose.q_near = urcl::Q{ toDouble(second_raw[0]), toDouble(second_raw[1]), toDouble(second_raw[2]), + toDouble(second_raw[3]), toDouble(second_raw[4]), toDouble(second_raw[5]) }; + } + return pose; + } + std::shared_ptr getMotionPrimitive() { TrajData spl = getData(); const double blend_radius = toDouble(spl.blend_radius_or_spline_type); - const double acceleration = toDouble(spl.acc[0]); - const double velocity = toDouble(spl.vel[0]); + // Third block (decoded into ``spl.acc``) is [velocity, acceleration, …] for non-spline moves. + const double velocity = toDouble(spl.acc[0]); + const double acceleration = toDouble(spl.acc[1]); const auto duration = std::chrono::microseconds(spl.goal_time); if (spl.motion_type == static_cast(control::MotionType::MOVEJ)) @@ -246,14 +261,14 @@ class TrajectoryPointInterfaceTest : public ::testing::Test } else if (spl.motion_type == static_cast(control::MotionType::MOVEJ_POSE)) { - const auto target = spl.pos; - return std::make_shared(urcl::MotionTarget{ toPose(target) }, blend_radius, duration, - acceleration, velocity); + return std::make_shared( + urcl::MotionTarget{ poseWithOptionalQNear(spl.pos, spl.vel, spl.acc) }, blend_radius, duration, + acceleration, velocity); } else if (spl.motion_type == static_cast(control::MotionType::MOVEL)) { - return std::make_shared(toPose(spl.pos), blend_radius, duration, acceleration, - velocity); + return std::make_shared(poseWithOptionalQNear(spl.pos, spl.vel, spl.acc), blend_radius, + duration, acceleration, velocity); } else if (spl.motion_type == static_cast(control::MotionType::MOVEL_JOINT)) { @@ -264,7 +279,8 @@ class TrajectoryPointInterfaceTest : public ::testing::Test } else if (spl.motion_type == static_cast(control::MotionType::MOVEP)) { - return std::make_shared(toPose(spl.pos), blend_radius, acceleration, velocity); + return std::make_shared(poseWithOptionalQNear(spl.pos, spl.vel, spl.acc), blend_radius, + acceleration, velocity); } else if (spl.motion_type == static_cast(control::MotionType::MOVEP_JOINT)) { @@ -308,12 +324,14 @@ class TrajectoryPointInterfaceTest : public ::testing::Test } else if (spl.motion_type == static_cast(control::MotionType::OPTIMOVEJ_POSE)) { - return std::make_shared(urcl::MotionTarget{ toPose(spl.pos) }, blend_radius, - acceleration, velocity); + return std::make_shared( + urcl::MotionTarget{ poseWithOptionalQNear(spl.pos, spl.vel, spl.acc) }, blend_radius, acceleration, + velocity); } else if (spl.motion_type == static_cast(control::MotionType::OPTIMOVEL)) { - return std::make_shared(toPose(spl.pos), blend_radius, acceleration, velocity); + return std::make_shared(poseWithOptionalQNear(spl.pos, spl.vel, spl.acc), + blend_radius, acceleration, velocity); } else if (spl.motion_type == static_cast(control::MotionType::OPTIMOVEL_JOINT)) { @@ -426,8 +444,8 @@ TEST_F(TrajectoryPointInterfaceTest, write_quintic_joint_spline) EXPECT_EQ(send_acc[4], ((double)received_data.acc[4]) / traj_point_interface_->MULT_JOINTSTATE); EXPECT_EQ(send_acc[5], ((double)received_data.acc[5]) / traj_point_interface_->MULT_JOINTSTATE); - // Goal time - EXPECT_EQ(send_goal_time, ((double)received_data.goal_time / traj_point_interface_->MULT_JOINTSTATE)); + // Goal time (segment duration, ``MULT_TIME`` on the wire) + EXPECT_EQ(send_goal_time, ((double)received_data.goal_time / traj_point_interface_->MULT_TIME)); // Spline type EXPECT_EQ(static_cast(control::TrajectorySplineType::SPLINE_QUINTIC), @@ -471,7 +489,7 @@ TEST_F(TrajectoryPointInterfaceTest, write_cubic_joint_spline) EXPECT_EQ(send_acc[5], ((double)received_data.acc[5]) / traj_point_interface_->MULT_JOINTSTATE); // Goal time - EXPECT_EQ(send_goal_time, ((double)received_data.goal_time) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_goal_time, ((double)received_data.goal_time) / traj_point_interface_->MULT_TIME); // Spline type EXPECT_EQ(static_cast(control::TrajectorySplineType::SPLINE_CUBIC), @@ -522,7 +540,7 @@ TEST_F(TrajectoryPointInterfaceTest, write_goal_time) traj_point_interface_->writeTrajectoryPoint(&send_positions, send_goal_time, 0, false); int32_t received_goal_time = client_->getGoalTime(); - EXPECT_EQ(send_goal_time, ((float)received_goal_time) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_goal_time, ((float)received_goal_time) / traj_point_interface_->MULT_TIME); } // Wire format: int32 microseconds (MULT_TIME). Duration must reach writeMotionPrimitive as seconds @@ -588,17 +606,11 @@ TEST_F(TrajectoryPointInterfaceTest, write_acceleration_velocity) float send_goal_time = 0.5; traj_point_interface_->writeTrajectoryPoint(&send_positions, send_move_acceleration, send_move_velocity, send_goal_time, 0, 0); - int32_t received_move_acceleration = client_->getAcceleration()[0]; - traj_point_interface_->writeTrajectoryPoint(&send_positions, send_move_acceleration, send_move_velocity, - send_goal_time, 0, 0); - int32_t received_move_velocity = client_->getVelocity()[0]; - traj_point_interface_->writeTrajectoryPoint(&send_positions, send_move_acceleration, send_move_velocity, - send_goal_time, 0, 0); - int32_t received_goal_time = client_->getGoalTime(); + Client::TrajData d = client_->getData(); - EXPECT_EQ(send_move_acceleration, ((float)received_move_acceleration) / traj_point_interface_->MULT_JOINTSTATE); - EXPECT_EQ(send_move_velocity, ((float)received_move_velocity) / traj_point_interface_->MULT_JOINTSTATE); - EXPECT_EQ(send_goal_time, ((float)received_goal_time) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_move_velocity, ((float)d.acc[0]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_move_acceleration, ((float)d.acc[1]) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_goal_time, ((float)d.goal_time) / traj_point_interface_->MULT_TIME); } TEST_F(TrajectoryPointInterfaceTest, write_blend_radius) @@ -608,7 +620,7 @@ TEST_F(TrajectoryPointInterfaceTest, write_blend_radius) traj_point_interface_->writeTrajectoryPoint(&send_positions, 0, send_blend_radius, false); int32_t received_blend_radius = client_->getBlendRadius(); - EXPECT_EQ(send_blend_radius, ((float)received_blend_radius) / traj_point_interface_->MULT_JOINTSTATE); + EXPECT_EQ(send_blend_radius, ((float)received_blend_radius) / traj_point_interface_->MULT_TIME); } TEST_F(TrajectoryPointInterfaceTest, write_cartesian) @@ -668,7 +680,7 @@ TEST_F(TrajectoryPointInterfaceTest, send_movej) TEST_F(TrajectoryPointInterfaceTest, send_movel) { - urcl::Pose send_positions = { 1.2, 3.1, 2.2, -3.4, -1.1, -1.2 }; + urcl::Pose send_positions(1.2, 3.1, 2.2, -3.4, -1.1, -1.2); double blend_radius = 0.5; double velocity = 0.6; double acceleration = 0.7; @@ -690,7 +702,7 @@ TEST_F(TrajectoryPointInterfaceTest, send_movel) TEST_F(TrajectoryPointInterfaceTest, send_movep) { - urcl::Pose send_positions = { 1.2, 3.1, 2.2, -3.4, -1.1, -1.2 }; + urcl::Pose send_positions(1.2, 3.1, 2.2, -3.4, -1.1, -1.2); double blend_radius = 0.5; double velocity = 0.6; double acceleration = 0.7; @@ -709,8 +721,8 @@ TEST_F(TrajectoryPointInterfaceTest, send_movep) TEST_F(TrajectoryPointInterfaceTest, send_movec) { - urcl::Pose send_target = { 1.2, 3.1, 2.2, -3.4, -1.1, -1.2 }; - urcl::Pose send_via = { 0.5, 0.6, 0.7, 0.8, 0.9, 1.0 }; + urcl::Pose send_target(1.2, 3.1, 2.2, -3.4, -1.1, -1.2); + urcl::Pose send_via(0.5, 0.6, 0.7, 0.8, 0.9, 1.0); double blend_radius = 0.5; double acceleration = 0.4; double velocity = 0.7; @@ -732,26 +744,88 @@ TEST_F(TrajectoryPointInterfaceTest, send_movec) TEST_F(TrajectoryPointInterfaceTest, send_movej_pose) { - urcl::Pose send_pose = { 0.1, -0.2, 0.3, 0.4, -0.5, 0.6 }; + const urcl::Pose expected_pose(0.1, -0.2, 0.3, 0.4, -0.5, 0.6); double blend_radius = 0.25; double velocity = 0.6; double acceleration = 0.7; auto duration = std::chrono::milliseconds(500); - auto primitive = std::make_shared(urcl::MotionTarget{ send_pose }, blend_radius, duration, - acceleration, velocity); + auto primitive = std::make_shared( + urcl::MotionTarget{ std::in_place_type, 0.1, -0.2, 0.3, 0.4, -0.5, 0.6 }, blend_radius, duration, + acceleration, velocity); traj_point_interface_->writeMotionPrimitive(primitive); auto received_primitive = client_->getMotionPrimitive(); EXPECT_EQ(received_primitive->type, control::MotionType::MOVEJ_POSE); EXPECT_EQ(std::get( std::static_pointer_cast(received_primitive)->getTarget().value()), - send_pose); + expected_pose); EXPECT_EQ(received_primitive->blend_radius, blend_radius); EXPECT_EQ(received_primitive->velocity, velocity); EXPECT_EQ(received_primitive->acceleration, acceleration); EXPECT_EQ(received_primitive->duration, duration); } +TEST_F(TrajectoryPointInterfaceTest, send_movej_pose_with_q_near_roundtrip) +{ + urcl::Pose expected_pose(0.1, -0.2, 0.3, 0.4, -0.5, 0.6); + expected_pose.q_near = urcl::Q{ -1.0, -2.0, 1.0, 0.5, 0.25, 0.0 }; + double blend_radius = 0.25; + double velocity = 0.6; + double acceleration = 0.7; + auto duration = std::chrono::milliseconds(500); + urcl::MotionTarget target{ std::in_place_type, 0.1, -0.2, 0.3, 0.4, -0.5, 0.6 }; + std::get(target).q_near = urcl::Q{ -1.0, -2.0, 1.0, 0.5, 0.25, 0.0 }; + auto primitive = + std::make_shared(std::move(target), blend_radius, duration, acceleration, velocity); + + traj_point_interface_->writeMotionPrimitive(primitive); + auto received_primitive = client_->getMotionPrimitive(); + EXPECT_EQ(received_primitive->type, control::MotionType::MOVEJ_POSE); + EXPECT_EQ(std::get( + std::static_pointer_cast(received_primitive)->getTarget().value()), + expected_pose); +} + +TEST_F(TrajectoryPointInterfaceTest, send_movel_with_q_near_roundtrip) +{ + urcl::Pose send_positions(1.2, 3.1, 2.2, -3.4, -1.1, -1.2); + send_positions.q_near = urcl::Q{ 0.11, -0.22, 0.33, 0.44, -0.55, 0.66 }; + double blend_radius = 0.5; + double velocity = 0.6; + double acceleration = 0.7; + auto duration = std::chrono::milliseconds(434); + auto primitive = + std::make_shared(send_positions, blend_radius, duration, acceleration, velocity); + + traj_point_interface_->writeMotionPrimitive(primitive); + auto received_primitive = client_->getMotionPrimitive(); + EXPECT_EQ(received_primitive->type, control::MotionType::MOVEL); + EXPECT_EQ(std::get( + std::static_pointer_cast(received_primitive)->getTarget().value()), + send_positions); +} + +TEST_F(TrajectoryPointInterfaceTest, send_optimovej_pose_with_q_near_roundtrip) +{ + const urcl::Pose expected_pose(0.1, -0.2, 0.3, 0.4, -0.5, 0.6); + double blend_radius = 0.1; + double acceleration_fraction = 0.4; + double velocity_fraction = 0.6; + urcl::MotionTarget target{ std::in_place_type, 0.1, -0.2, 0.3, 0.4, -0.5, 0.6 }; + std::get(target).q_near = urcl::Q{ 0.01, 0.02, -0.03, 0.04, -0.05, 0.06 }; + auto primitive = std::make_shared(std::move(target), blend_radius, acceleration_fraction, + velocity_fraction); + + traj_point_interface_->writeMotionPrimitive(primitive); + auto received_primitive = client_->getMotionPrimitive(); + EXPECT_EQ(received_primitive->type, control::MotionType::OPTIMOVEJ_POSE); + urcl::Pose expected_with_q = expected_pose; + expected_with_q.q_near = urcl::Q{ 0.01, 0.02, -0.03, 0.04, -0.05, 0.06 }; + EXPECT_EQ(std::get( + std::static_pointer_cast(received_primitive)->getTarget().value()), + expected_with_q); +} + TEST_F(TrajectoryPointInterfaceTest, send_movel_joint) { urcl::Q send_joints{ 1.2, 3.1, 2.2, -3.4, -1.1, -1.2 }; @@ -822,7 +896,7 @@ TEST_F(TrajectoryPointInterfaceTest, send_movec_joint_joint) TEST_F(TrajectoryPointInterfaceTest, send_movec_pose_joint) { // via is a Pose, target is a Q - urcl::Pose send_via{ 0.1, -0.2, 0.3, 0.4, -0.5, 0.6 }; + urcl::Pose send_via(0.1, -0.2, 0.3, 0.4, -0.5, 0.6); urcl::Q send_target{ 1.2, 3.1, 2.2, -3.4, -1.1, -1.2 }; double blend_radius = 0.25; double acceleration = 0.7; @@ -848,7 +922,7 @@ TEST_F(TrajectoryPointInterfaceTest, send_movec_joint_pose) { // via is a Q, target is a Pose urcl::Q send_via{ 0.5, 0.6, 0.7, 0.8, 0.9, 1.0 }; - urcl::Pose send_target{ 1.2, 3.1, 2.2, -3.4, -1.1, -1.2 }; + urcl::Pose send_target(1.2, 3.1, 2.2, -3.4, -1.1, -1.2); double blend_radius = 0.25; double acceleration = 0.7; double velocity = 0.5; @@ -871,19 +945,20 @@ TEST_F(TrajectoryPointInterfaceTest, send_movec_joint_pose) TEST_F(TrajectoryPointInterfaceTest, send_optimovej_pose) { - urcl::Pose send_pose{ 0.1, -0.2, 0.3, 0.4, -0.5, 0.6 }; + const urcl::Pose expected_pose(0.1, -0.2, 0.3, 0.4, -0.5, 0.6); double blend_radius = 0.1; double acceleration_fraction = 0.4; double velocity_fraction = 0.6; - auto primitive = std::make_shared(urcl::MotionTarget{ send_pose }, blend_radius, - acceleration_fraction, velocity_fraction); + auto primitive = std::make_shared( + urcl::MotionTarget{ std::in_place_type, 0.1, -0.2, 0.3, 0.4, -0.5, 0.6 }, blend_radius, + acceleration_fraction, velocity_fraction); traj_point_interface_->writeMotionPrimitive(primitive); auto received_primitive = client_->getMotionPrimitive(); EXPECT_EQ(received_primitive->type, control::MotionType::OPTIMOVEJ_POSE); EXPECT_EQ(std::get( std::static_pointer_cast(received_primitive)->getTarget().value()), - send_pose); + expected_pose); EXPECT_EQ(received_primitive->blend_radius, blend_radius); EXPECT_EQ(received_primitive->velocity, velocity_fraction); EXPECT_EQ(received_primitive->acceleration, acceleration_fraction); From 76ed58d345e6f57d472dd0e1c5ef8a21c29ade2e Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 4 May 2026 12:02:07 +0200 Subject: [PATCH 19/33] Apply suggestions from code review MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Rune Søe-Knudsen <41109954+urrsk@users.noreply.github.com> --- doc/architecture/instruction_executor.rst | 3 ++- doc/architecture/trajectory_point_interface.rst | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/doc/architecture/instruction_executor.rst b/doc/architecture/instruction_executor.rst index 1f249ca96..59113bbe4 100644 --- a/doc/architecture/instruction_executor.rst +++ b/doc/architecture/instruction_executor.rst @@ -33,7 +33,8 @@ Every motion function comes in two flavours: linear tool-space motion towards the pose implied by a joint configuration. An ``urcl::Pose`` may optionally carry ``q_near`` (``std::optional``), a joint configuration -hint for IK when the pose is sent over the trajectory interface; see :ref:`trajectory_point_interface`. +hint for inverse kinematic solver to select the desired joint angle solution, +when the pose is sent over the trajectory interface; see :ref:`trajectory_point_interface`. The Instruction Executor uses the :ref:`trajectory_point_interface` and the :ref:`reverse_interface` diff --git a/doc/architecture/trajectory_point_interface.rst b/doc/architecture/trajectory_point_interface.rst index 2a08a2820..f16900d84 100644 --- a/doc/architecture/trajectory_point_interface.rst +++ b/doc/architecture/trajectory_point_interface.rst @@ -105,7 +105,7 @@ representations in 21 datafields. The data fields have the following meaning: - 11: MOVEC_POSE_JOINT – ``movec`` with joint via and pose target - 12: OPTIMOVEJ_POSE – ``optimovej`` to a pose target - 13: OPTIMOVEL_JOINT – ``optimovel`` to the pose implied by a joint target - - 51: SPLINE + - 51: SPLINE - Cubic or quintic spline. ===== ===== where From 0ccc889bff51012cfd269410fc2569914b02fc07 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 4 May 2026 12:06:58 +0200 Subject: [PATCH 20/33] Add constructor with q_near --- include/ur_client_library/types.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/include/ur_client_library/types.h b/include/ur_client_library/types.h index c5780febb..bfc92205a 100644 --- a/include/ur_client_library/types.h +++ b/include/ur_client_library/types.h @@ -76,6 +76,11 @@ struct Pose : x(x), y(y), z(z), rx(rx), ry(ry), rz(rz), q_near(std::nullopt) { } + Pose(const double x, const double y, const double z, const double rx, const double ry, const double rz, + const Q& q_near) + : x(x), y(y), z(z), rx(rx), ry(ry), rz(rz), q_near(q_near) + { + } double x; double y; double z; From 00468670d5e7df43129f6cc9aa14eec319d334ec Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 4 May 2026 15:35:28 +0200 Subject: [PATCH 21/33] Fix indentation --- resources/external_control.urscript | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/resources/external_control.urscript b/resources/external_control.urscript index ba3abf3a5..8cb6b51bc 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -586,7 +586,7 @@ thread trajectoryThread(): if has_qnear: target_q = get_inverse_kin(target_pose, second_block) end - movej(target_q, a = acceleration, v = velocity, t = tmptime, r = blend_radius) + movej(target_q, a = acceleration, v = velocity, t = tmptime, r = blend_radius) # reset old acceleration From 1f67187c959eaf8da42d1f35fea54a9ad3f9b80a Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 4 May 2026 15:52:11 +0200 Subject: [PATCH 22/33] Script code update --- resources/external_control.urscript | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 8cb6b51bc..e484dbbc4 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -573,7 +573,6 @@ thread trajectoryThread(): # MoveJ point if raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEJ: - textmsg("Moving to joint position: ", q) movej(q, a = acceleration, v = velocity, t = tmptime, r = blend_radius) # reset old acceleration @@ -581,10 +580,11 @@ thread trajectoryThread(): spline_qd = [0, 0, 0, 0, 0, 0] elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEJ_POSE: target_pose = p[q[0], q[1], q[2], q[3], q[4], q[5]] - textmsg("Moving to pose: ", target_pose) - target_q = get_inverse_kin(target_pose, get_target_joint_positions()) + target_q = get_target_joint_positions() if has_qnear: target_q = get_inverse_kin(target_pose, second_block) + else: + target_q = get_inverse_kin(target_pose, get_target_joint_positions()) end movej(target_q, a = acceleration, v = velocity, t = tmptime, r = blend_radius) From 2fab00aeebcd1026ae9a86be9f4a47273eb06fb4 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 4 May 2026 15:56:32 +0200 Subject: [PATCH 23/33] Do not pass goal_time as blend_radius --- examples/instruction_executor.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/examples/instruction_executor.cpp b/examples/instruction_executor.cpp index e40a45ffb..2d408aaae 100644 --- a/examples/instruction_executor.cpp +++ b/examples/instruction_executor.cpp @@ -128,16 +128,16 @@ int main(int argc, char* argv[]) instruction_executor->moveJ(urcl::Pose{ -0.0203, 0.363, 0.559, 0.68, -1.083, -2.076 }, 0.1, 0.1, goal_time_sec); // moveP can also be called with brace-init (interpreted as pose) or explicitly using a Pose or Q - instruction_executor->moveP({ -0.2, 0.363, 0.559, 0.68, -1.083, -2.076 }, 0.1, 0.1, goal_time_sec); - instruction_executor->moveP(urcl::Pose{ -0.0203, 0.303, 0.559, 0.68, -1.083, -2.076 }, 0.1, 0.1, goal_time_sec); - instruction_executor->moveP(urcl::Q{ -1.57, -1.83, 1.707, -0.833, 0.782, 0.479 }, 0.1, 0.1, goal_time_sec); + instruction_executor->moveP({ -0.2, 0.363, 0.559, 0.68, -1.083, -2.076 }); + instruction_executor->moveP(urcl::Pose{ -0.0203, 0.303, 0.559, 0.68, -1.083, -2.076 }); + instruction_executor->moveP(urcl::Q{ -1.57, -1.83, 1.707, -0.833, 0.782, 0.479 }); // For moveC via and target can be a Pose or Q. When brace-init style lists are given, values are // interpreted as Pose. instruction_executor->moveC(urcl::Pose{ -0.1, 0.463, 0.559, 0.68, -1.083, -2.076 }, - urcl::Pose{ -0.0203, 0.303, 0.559, 0.68, -1.083, -2.076 }, 0.1, 0.1, goal_time_sec); + urcl::Pose{ -0.0203, 0.303, 0.559, 0.68, -1.083, -2.076 }); instruction_executor->moveC(urcl::Pose{ -0.1, 0.463, 0.559, 0.68, -1.083, -2.076 }, - urcl::Q{ -1.57, -1.83, 1.707, -0.833, 0.782, 0.479 }, 0.1, 0.1, goal_time_sec); + urcl::Q{ -1.57, -1.83, 1.707, -0.833, 0.782, 0.479 }); // For optimove functions, the same target rules as for moveJ and moveL apply. instruction_executor->optimoveJ({ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 }, 1.0, 1.0); From d8e38247f84a9aaa59e9e22bd05669d1d7626f97 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 4 May 2026 16:15:31 +0200 Subject: [PATCH 24/33] Add test of primitive types --- tests/CMakeLists.txt | 10 ++ tests/test_motion_primitives.cpp | 246 +++++++++++++++++++++++++++++++ tests/test_types.cpp | 165 +++++++++++++++++++++ 3 files changed, 421 insertions(+) create mode 100644 tests/test_motion_primitives.cpp create mode 100644 tests/test_types.cpp diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 0aff4cdb4..7ac7e214b 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -271,3 +271,13 @@ add_executable(helpers_tests test_helpers.cpp) target_link_libraries(helpers_tests PRIVATE ur_client_library::urcl GTest::gtest_main) gtest_add_tests(TARGET helpers_tests ) + +add_executable(types_tests test_types.cpp) +target_link_libraries(types_tests PRIVATE ur_client_library::urcl GTest::gtest_main) +gtest_add_tests(TARGET types_tests +) + +add_executable(motion_primitives_tests test_motion_primitives.cpp) +target_link_libraries(motion_primitives_tests PRIVATE ur_client_library::urcl GTest::gtest_main) +gtest_add_tests(TARGET motion_primitives_tests +) diff --git a/tests/test_motion_primitives.cpp b/tests/test_motion_primitives.cpp new file mode 100644 index 000000000..dcc8ccad1 --- /dev/null +++ b/tests/test_motion_primitives.cpp @@ -0,0 +1,246 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2026 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 +#include + +#include + +using urcl::MotionTarget; +using urcl::Pose; +using urcl::Q; +using urcl::vector6d_t; +using urcl::control::MotionPrimitive; +using urcl::control::MotionType; +using urcl::control::motionTypeToString; +using urcl::control::MoveCPrimitive; +using urcl::control::MoveJPrimitive; +using urcl::control::MoveLPrimitive; +using urcl::control::MovePPrimitive; +using urcl::control::OptimoveJPrimitive; +using urcl::control::OptimoveLPrimitive; +using urcl::control::SplinePrimitive; +using urcl::control::TrajectorySplineType; + +namespace +{ +vector6d_t zeroJoints() +{ + return { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; +} + +Pose samplePose(const double x = 1.0) +{ + return Pose(x, 2.0, 3.0, 0.1, 0.2, 0.3); +} +} // namespace + +TEST(MotionTypeToStringTest, returns_expected_string_for_each_known_type) +{ + EXPECT_EQ(motionTypeToString(MotionType::MOVEJ), "MOVEJ"); + EXPECT_EQ(motionTypeToString(MotionType::MOVEL), "MOVEL"); + EXPECT_EQ(motionTypeToString(MotionType::MOVEP), "MOVEP"); + EXPECT_EQ(motionTypeToString(MotionType::MOVEC), "MOVEC"); + EXPECT_EQ(motionTypeToString(MotionType::OPTIMOVEJ), "OPTIMOVEJ"); + EXPECT_EQ(motionTypeToString(MotionType::OPTIMOVEL), "OPTIMOVEL"); + EXPECT_EQ(motionTypeToString(MotionType::MOVEJ_POSE), "MOVEJ_POSE"); + EXPECT_EQ(motionTypeToString(MotionType::MOVEL_JOINT), "MOVEL_JOINT"); + EXPECT_EQ(motionTypeToString(MotionType::MOVEP_JOINT), "MOVEP_JOINT"); + EXPECT_EQ(motionTypeToString(MotionType::MOVEC_JOINT), "MOVEC_JOINT"); + EXPECT_EQ(motionTypeToString(MotionType::MOVEC_JOINT_POSE), "MOVEC_JOINT_POSE"); + EXPECT_EQ(motionTypeToString(MotionType::MOVEC_POSE_JOINT), "MOVEC_POSE_JOINT"); + EXPECT_EQ(motionTypeToString(MotionType::OPTIMOVEJ_POSE), "OPTIMOVEJ_POSE"); + EXPECT_EQ(motionTypeToString(MotionType::OPTIMOVEL_JOINT), "OPTIMOVEL_JOINT"); + EXPECT_EQ(motionTypeToString(MotionType::SPLINE), "SPLINE"); +} + +TEST(MotionTypeToStringTest, returns_unknown_for_explicit_unknown_value) +{ + EXPECT_EQ(motionTypeToString(MotionType::UNKNOWN), "UNKNOWN"); +} + +TEST(MotionTypeToStringTest, returns_unknown_for_out_of_range_value) +{ + const auto out_of_range = static_cast(static_cast(200)); + EXPECT_EQ(motionTypeToString(out_of_range), "UNKNOWN"); +} + +TEST(MotionPrimitiveTest, validate_accepts_defaults_and_non_negative_parameters) +{ + MotionPrimitive mp; + EXPECT_TRUE(mp.validate()); + EXPECT_DOUBLE_EQ(mp.blend_radius, 0.0); + EXPECT_DOUBLE_EQ(mp.acceleration, 1.4); + EXPECT_DOUBLE_EQ(mp.velocity, 1.04); +} + +TEST(MotionPrimitiveTest, validate_rejects_negative_blend_radius_acceleration_or_velocity) +{ + MotionPrimitive bad_blend(-1.0); + EXPECT_FALSE(bad_blend.validate()); + + MotionPrimitive bad_acc(0, std::chrono::milliseconds(0), -0.1); + EXPECT_FALSE(bad_acc.validate()); + + MotionPrimitive bad_vel(0, std::chrono::milliseconds(0), 1.4, -1.0); + EXPECT_FALSE(bad_vel.validate()); +} + +TEST(MoveJPrimitiveTest, joint_vector_constructor_sets_movej_and_target) +{ + const vector6d_t q{ { 0.1, 0.2, 0.3, 0.4, 0.5, 0.6 } }; + MoveJPrimitive mj(q); + EXPECT_EQ(mj.type, MotionType::MOVEJ); + ASSERT_TRUE(mj.getTarget().has_value()); + ASSERT_TRUE(std::holds_alternative(*mj.getTarget())); + EXPECT_EQ(std::get(*mj.getTarget()), Q(q)); +} + +TEST(MoveJPrimitiveTest, motion_target_q_vs_pose_sets_motion_type) +{ + MoveJPrimitive from_q(MotionTarget(Q(0, 0, 0, 0, 0, 0))); + EXPECT_EQ(from_q.type, MotionType::MOVEJ); + + const MotionTarget pose_target{ samplePose() }; + MoveJPrimitive movej_from_pose(pose_target); + EXPECT_EQ(movej_from_pose.type, MotionType::MOVEJ_POSE); + ASSERT_TRUE(movej_from_pose.getTarget().has_value()); + ASSERT_TRUE(std::holds_alternative(*movej_from_pose.getTarget())); +} + +TEST(MoveLPrimitiveTest, pose_and_motion_target_variants_set_motion_type) +{ + MoveLPrimitive ml(samplePose()); + EXPECT_EQ(ml.type, MotionType::MOVEL); + ASSERT_TRUE(ml.getTarget().has_value()); + ASSERT_TRUE(std::holds_alternative(*ml.getTarget())); + + MoveLPrimitive mlj(MotionTarget(Q(1, 2, 3, 4, 5, 6))); + EXPECT_EQ(mlj.type, MotionType::MOVEL_JOINT); + ASSERT_TRUE(std::holds_alternative(*mlj.getTarget())); +} + +TEST(MovePPrimitiveTest, pose_and_motion_target_variants_set_motion_type) +{ + MovePPrimitive mp(samplePose()); + EXPECT_EQ(mp.type, MotionType::MOVEP); + + MovePPrimitive mpj(MotionTarget(Q(0.1, 0.2, 0.3, 0.4, 0.5, 0.6))); + EXPECT_EQ(mpj.type, MotionType::MOVEP_JOINT); +} + +TEST(MoveCPrimitiveTest, recomputes_motion_type_for_via_target_combinations) +{ + const Pose via_p = samplePose(0.0); + const Pose target_p = samplePose(10.0); + const Q via_q(0, 0, 0, 0, 0, 0); + const Q target_q(1, 1, 1, 1, 1, 1); + + MoveCPrimitive pp(via_p, target_p); + EXPECT_EQ(pp.type, MotionType::MOVEC); + + MoveCPrimitive qq(via_q, target_q); + EXPECT_EQ(qq.type, MotionType::MOVEC_JOINT); + + MoveCPrimitive qp(via_q, target_p); + EXPECT_EQ(qp.type, MotionType::MOVEC_POSE_JOINT); + + MoveCPrimitive pq(via_p, target_q); + EXPECT_EQ(pq.type, MotionType::MOVEC_JOINT_POSE); +} + +TEST(MoveCPrimitiveTest, set_via_and_set_target_update_type_and_get_via) +{ + MoveCPrimitive mc(samplePose(0.0), samplePose(1.0)); + EXPECT_EQ(mc.type, MotionType::MOVEC); + + mc.setVia(MotionTarget(Q(0, 0, 0, 0, 0, 0))); + mc.setTarget(MotionTarget(Q(1, 1, 1, 1, 1, 1))); + EXPECT_EQ(mc.type, MotionType::MOVEC_JOINT); + + ASSERT_TRUE(std::holds_alternative(mc.getVia())); + EXPECT_EQ(std::get(mc.getVia()), Q(0, 0, 0, 0, 0, 0)); + + EXPECT_EQ(mc.mode, 0); +} + +TEST(SplinePrimitiveTest, type_spline_and_getSplineType_cubic_vs_quintic) +{ + const vector6d_t pos = zeroJoints(); + const vector6d_t vel = zeroJoints(); + + SplinePrimitive cubic(pos, vel, std::nullopt); + EXPECT_EQ(cubic.type, MotionType::SPLINE); + EXPECT_EQ(cubic.getSplineType(), TrajectorySplineType::SPLINE_CUBIC); + EXPECT_TRUE(cubic.validate()); + + const vector6d_t acc = zeroJoints(); + SplinePrimitive quintic(pos, vel, acc); + EXPECT_EQ(quintic.getSplineType(), TrajectorySplineType::SPLINE_QUINTIC); + EXPECT_TRUE(quintic.validate()); +} + +TEST(OptimoveJPrimitiveTest, validate_fraction_range_and_motion_types) +{ + OptimoveJPrimitive ok(zeroJoints(), 0.0, 0.3, 0.3); + EXPECT_TRUE(ok.validate()); + EXPECT_EQ(ok.type, MotionType::OPTIMOVEJ); + + const MotionTarget pose_target{ samplePose() }; + OptimoveJPrimitive optj_from_pose(pose_target); + EXPECT_EQ(optj_from_pose.type, MotionType::OPTIMOVEJ_POSE); + + OptimoveJPrimitive bad_acc(zeroJoints(), 0.0, 0.0, 0.5); + EXPECT_FALSE(bad_acc.validate()); + + OptimoveJPrimitive bad_acc_high(zeroJoints(), 0.0, 1.01, 0.5); + EXPECT_FALSE(bad_acc_high.validate()); + + OptimoveJPrimitive bad_vel(zeroJoints(), 0.0, 0.5, 0.0); + EXPECT_FALSE(bad_vel.validate()); + + OptimoveJPrimitive bad_vel_high(zeroJoints(), 0.0, 0.5, 1.01); + EXPECT_FALSE(bad_vel_high.validate()); +} + +TEST(OptimoveLPrimitiveTest, validate_fraction_range_and_motion_types) +{ + OptimoveLPrimitive ok(samplePose(), 0.0, 0.3, 0.3); + EXPECT_TRUE(ok.validate()); + EXPECT_EQ(ok.type, MotionType::OPTIMOVEL); + + OptimoveLPrimitive from_q(MotionTarget(Q(0, 0, 0, 0, 0, 0))); + EXPECT_EQ(from_q.type, MotionType::OPTIMOVEL_JOINT); + + OptimoveLPrimitive bad(samplePose(), 0.0, 0.0, 0.5); + EXPECT_FALSE(bad.validate()); +} diff --git a/tests/test_types.cpp b/tests/test_types.cpp new file mode 100644 index 000000000..8d2217e59 --- /dev/null +++ b/tests/test_types.cpp @@ -0,0 +1,165 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2026 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 +#include + +#include + +using namespace urcl; + +TEST(TestTypes, Q_constructors_and_equality) +{ + const Q from_doubles{ 0.1, 0.2, 0.3, 0.4, 0.5, 0.6 }; + const vector6d_t arr{ { 0.1, 0.2, 0.3, 0.4, 0.5, 0.6 } }; + const Q from_array(arr); + + EXPECT_EQ(from_doubles, from_array); + EXPECT_EQ(from_doubles.values.size(), 6u); + EXPECT_EQ(from_array.values.size(), 6u); + + const Q different{ 1.0, 0.2, 0.3, 0.4, 0.5, 0.6 }; + EXPECT_FALSE(from_doubles == different); +} + +TEST(TestTypes, Q_equals_vector6d) +{ + const Q q{ 1.0, 2.0, 3.0, 4.0, 5.0, 6.0 }; + const vector6d_t match{ { 1.0, 2.0, 3.0, 4.0, 5.0, 6.0 } }; + const vector6d_t mismatch{ { 1.0, 2.0, 3.0, 4.0, 5.0, 7.0 } }; + + EXPECT_TRUE(q == match); + EXPECT_FALSE(q == mismatch); +} + +TEST(TestTypes, Pose_default_and_constructors) +{ + Pose default_pose; + EXPECT_DOUBLE_EQ(default_pose.x, 0.0); + EXPECT_DOUBLE_EQ(default_pose.y, 0.0); + EXPECT_DOUBLE_EQ(default_pose.z, 0.0); + EXPECT_DOUBLE_EQ(default_pose.rx, 0.0); + EXPECT_DOUBLE_EQ(default_pose.ry, 0.0); + EXPECT_DOUBLE_EQ(default_pose.rz, 0.0); + EXPECT_FALSE(default_pose.q_near.has_value()); + + const Pose cartesian(1.0, 2.0, 3.0, 0.1, 0.2, 0.3); + EXPECT_DOUBLE_EQ(cartesian.x, 1.0); + EXPECT_DOUBLE_EQ(cartesian.y, 2.0); + EXPECT_DOUBLE_EQ(cartesian.z, 3.0); + EXPECT_DOUBLE_EQ(cartesian.rx, 0.1); + EXPECT_DOUBLE_EQ(cartesian.ry, 0.2); + EXPECT_DOUBLE_EQ(cartesian.rz, 0.3); + EXPECT_FALSE(cartesian.q_near.has_value()); + + const Q hint{ 0.0, -0.5, 1.0, 0.0, 0.5, 0.0 }; + const Pose with_hint(10.0, 20.0, 30.0, 1.0, 2.0, 3.0, hint); + ASSERT_TRUE(with_hint.q_near.has_value()); + EXPECT_EQ(*with_hint.q_near, hint); +} + +TEST(TestTypes, Pose_equality) +{ + const Pose a(1.0, 2.0, 3.0, 0.1, 0.2, 0.3); + const Pose b(1.0, 2.0, 3.0, 0.1, 0.2, 0.3); + const Pose c(1.1, 2.0, 3.0, 0.1, 0.2, 0.3); + + const Q q{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + const Pose with_q(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, q); + const Pose without_q_near(1.0, 2.0, 3.0, 0.1, 0.2, 0.3); + + EXPECT_TRUE(a == b); + EXPECT_FALSE(a == c); + EXPECT_FALSE(with_q == without_q_near); + + const Pose with_same_q(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, q); + EXPECT_TRUE(with_q == with_same_q); + + const Q q2{ 1.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + const Pose different_hint(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, q2); + EXPECT_FALSE(with_q == different_hint); +} + +TEST(TestTypes, MotionTarget_variant) +{ + const MotionTarget joint_side = Q{ 0.0, 0.1, 0.2, 0.3, 0.4, 0.5 }; + ASSERT_TRUE(std::holds_alternative(joint_side)); + EXPECT_EQ(std::get(joint_side), Q(0.0, 0.1, 0.2, 0.3, 0.4, 0.5)); + + const MotionTarget cart_side = Pose(1.0, 2.0, 3.0, 0.0, 0.0, 0.0); + ASSERT_TRUE(std::holds_alternative(cart_side)); + const Pose& p = std::get(cart_side); + EXPECT_DOUBLE_EQ(p.x, 1.0); + EXPECT_DOUBLE_EQ(p.z, 3.0); +} + +TEST(TestTypes, array_stream_operator) +{ + const vector3d_t v3{ { 1.0, 2.5, -3.0 } }; + const vector6int32_t v6i{ { -1, 0, 42, 3, 4, 5 } }; + + std::ostringstream out3; + out3 << v3; + EXPECT_EQ(out3.str(), "[1, 2.5, -3]"); + + std::ostringstream out6; + out6 << v6i; + EXPECT_EQ(out6.str(), "[-1, 0, 42, 3, 4, 5]"); +} + +TEST(TestTypes, toUnderlying) +{ + enum class Small : uint8_t + { + X = 7, + Y = 200 + }; + static_assert(toUnderlying(Small::X) == 7, ""); + EXPECT_EQ(toUnderlying(Small::X), 7u); + EXPECT_EQ(toUnderlying(Small::Y), 200u); +} + +TEST(TestTypes, HandlerFunction_equality_uses_id_only) +{ + int calls_a = 0; + int calls_b = 0; + HandlerFunction ha(1, [&calls_a]() { ++calls_a; }); + HandlerFunction hb(1, [&calls_b]() { ++calls_b; }); + HandlerFunction hc(2, [&calls_a]() { ++calls_a; }); + + EXPECT_TRUE(ha == hb); + EXPECT_FALSE(ha == hc); + + ha.function(); + EXPECT_EQ(calls_a, 1); + EXPECT_EQ(calls_b, 0); +} From b0fed74ba944269c026ee71a877db13192179464 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 4 May 2026 20:43:25 +0200 Subject: [PATCH 25/33] Use central macro definition for deprecation suppression --- include/ur_client_library/helpers.h | 28 ++++++++++++++++++++++++++ src/control/motion_primitives.cpp | 8 ++++---- src/ur/ur_driver.cpp | 31 +++++------------------------ tests/test_primary_client.cpp | 5 ++--- tests/test_reverse_interface.cpp | 15 +++----------- tests/test_rtde_client.cpp | 7 +++---- tests/test_rtde_parser.cpp | 14 ++----------- tests/test_tcp_socket.cpp | 9 ++------- tests/test_ur_driver.cpp | 11 ++++------ 9 files changed, 53 insertions(+), 75 deletions(-) diff --git a/include/ur_client_library/helpers.h b/include/ur_client_library/helpers.h index e50a0f8d1..ca5f94e79 100644 --- a/include/ur_client_library/helpers.h +++ b/include/ur_client_library/helpers.h @@ -66,6 +66,34 @@ static inline int sched_get_priority_max(int policy) #endif // _WIN32 +/*! + * \file + * \brief Portable helpers to temporarily silence deprecation diagnostics. + * + * Use \ref URCL_SILENCE_DEPRECATED_BEGIN before and \ref URCL_SILENCE_DEPRECATED_END after the + * code that must call deprecated APIs (e.g. deprecated library methods implemented for backward + * compatibility). Expands to nothing on unsupported compilers. + * + * Example: + * \code + * URCL_SILENCE_DEPRECATED_BEGIN + * legacy_call(); + * URCL_SILENCE_DEPRECATED_END + * \endcode + */ + +#if defined(_MSC_VER) +# define URCL_SILENCE_DEPRECATED_BEGIN __pragma(warning(push)) __pragma(warning(disable : 4996)) +# define URCL_SILENCE_DEPRECATED_END __pragma(warning(pop)) +#elif defined(__GNUC__) +# define URCL_SILENCE_DEPRECATED_BEGIN \ + _Pragma("GCC diagnostic push") _Pragma("GCC diagnostic ignored \"-Wdeprecated-declarations\"") +# define URCL_SILENCE_DEPRECATED_END _Pragma("GCC diagnostic pop") +#else +# define URCL_SILENCE_DEPRECATED_BEGIN +# define URCL_SILENCE_DEPRECATED_END +#endif + namespace urcl { bool setFiFoScheduling(pthread_t& thread, const int priority); diff --git a/src/control/motion_primitives.cpp b/src/control/motion_primitives.cpp index 2a8c857b1..6fe8bfbe3 100644 --- a/src/control/motion_primitives.cpp +++ b/src/control/motion_primitives.cpp @@ -28,13 +28,13 @@ // POSSIBILITY OF SUCH DAMAGE. // -- END LICENSE BLOCK ------------------------------------------------ +#include #include #include namespace urcl::control { -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" +URCL_SILENCE_DEPRECATED_BEGIN bool MotionPrimitive::validate() const { @@ -164,7 +164,6 @@ void MoveJPrimitive::setTarget(const MotionTarget& target) if constexpr (std::is_same_v) { type = MotionType::MOVEJ_POSE; - target_joint_configuration.fill(0); } else if constexpr (std::is_same_v) @@ -426,6 +425,7 @@ void OptimoveLPrimitive::setTarget(const MotionTarget& target) }, target); } -#pragma GCC diagnostic pop + +URCL_SILENCE_DEPRECATED_END } // namespace urcl::control diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index f77a9f5c8..121352634 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -35,7 +35,6 @@ #include "ur_client_library/rtde/rtde_client.h" #include "ur_client_library/control/script_reader.h" #include "ur_client_library/exceptions.h" -#include "ur_client_library/helpers.h" #include "ur_client_library/primary/primary_parser.h" #include "ur_client_library/helpers.h" #include @@ -188,13 +187,7 @@ void UrDriver::init(const UrDriverConfiguration& config) URCL_LOG_DEBUG("Initialization done"); } -#ifdef __GNUC__ -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#elif defined(_MSC_VER) -# pragma warning(push) -# pragma warning(disable : 4996) -#endif +URCL_SILENCE_DEPRECATED_BEGIN std::unique_ptr urcl::UrDriver::getDataPackage() { // This can take one of two values, 0ms or 100ms. The large timeout is for when the robot is commanding the control @@ -204,11 +197,7 @@ std::unique_ptr urcl::UrDriver::getDataPackage() return rtde_client_->getDataPackage(timeout); } -#ifdef __GNUC__ -# pragma GCC diagnostic pop -#elif defined(_MSC_VER) -# pragma warning(pop) -#endif +URCL_SILENCE_DEPRECATED_END bool UrDriver::getDataPackage(rtde_interface::DataPackage& data_package) { @@ -708,20 +697,10 @@ void UrDriver::setKeepaliveCount(const uint32_t count) "set the " "read timeout in the write commands directly. This keepalive count will overwrite the timeout passed " "to the write functions."); -// TODO: Remove 2027-05 -#ifdef _MSC_VER -# pragma warning(push) -# pragma warning(disable : 4996) -#else -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#endif + // TODO: Remove 2027-05 + URCL_SILENCE_DEPRECATED_BEGIN reverse_interface_->setKeepaliveCount(count); -#ifdef _MSC_VER -# pragma warning(pop) -#else -# pragma GCC diagnostic pop -#endif + URCL_SILENCE_DEPRECATED_END } void UrDriver::resetRTDEClient(const std::vector& output_recipe, diff --git a/tests/test_primary_client.cpp b/tests/test_primary_client.cpp index cf129bc84..8c2a1595c 100644 --- a/tests/test_primary_client.cpp +++ b/tests/test_primary_client.cpp @@ -103,10 +103,9 @@ TEST_F(PrimaryClientTest, start_communication_succeeds) TEST_F(PrimaryClientTest, add_and_remove_consumer) { -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + URCL_SILENCE_DEPRECATED_BEGIN auto calibration_consumer = std::make_shared("test"); -#pragma GCC diagnostic pop + URCL_SILENCE_DEPRECATED_END client_->addPrimaryConsumer(calibration_consumer); diff --git a/tests/test_reverse_interface.cpp b/tests/test_reverse_interface.cpp index 80595db5d..512e42f68 100644 --- a/tests/test_reverse_interface.cpp +++ b/tests/test_reverse_interface.cpp @@ -33,6 +33,7 @@ #include #include #include +#include "ur_client_library/helpers.h" #include "ur_client_library/log.h" using namespace urcl; @@ -484,19 +485,9 @@ TEST_F(ReverseInterfaceTest, deprecated_set_keep_alive_count) // Test that it works to set the keepalive count using the deprecated function int keep_alive_count = 10; -#ifdef _MSC_VER -# pragma warning(push) -# pragma warning(disable : 4996) -#else -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#endif + URCL_SILENCE_DEPRECATED_BEGIN reverse_interface_->setKeepaliveCount(keep_alive_count); -#ifdef _MSC_VER -# pragma warning(pop) -#else -# pragma GCC diagnostic pop -#endif + URCL_SILENCE_DEPRECATED_END int32_t expected_read_timeout = 20 * keep_alive_count; urcl::vector6d_t pos = { 0, 0, 0, 0, 0, 0 }; diff --git a/tests/test_rtde_client.cpp b/tests/test_rtde_client.cpp index 19190a449..557465726 100644 --- a/tests/test_rtde_client.cpp +++ b/tests/test_rtde_client.cpp @@ -37,12 +37,12 @@ #include #include "ur_client_library/comm/tcp_server.h" #include "ur_client_library/exceptions.h" +#include "ur_client_library/helpers.h" #include #include #include "fake_rtde_server.h" -#include "ur_client_library/helpers.h" #include "ur_client_library/log.h" using namespace urcl; @@ -301,10 +301,9 @@ TEST_F(RTDEClientTest, get_data_package_w_background_deprecated) // Test that we can receive a package and extract data from the received package const std::chrono::milliseconds read_timeout{ 100 }; -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + URCL_SILENCE_DEPRECATED_BEGIN std::unique_ptr data_pkg = client_->getDataPackage(read_timeout); -#pragma GCC diagnostic pop + URCL_SILENCE_DEPRECATED_END if (data_pkg == nullptr) { std::cout << "Failed to get data package from robot" << std::endl; diff --git a/tests/test_rtde_parser.cpp b/tests/test_rtde_parser.cpp index 3c11b96d4..20a2a0eff 100644 --- a/tests/test_rtde_parser.cpp +++ b/tests/test_rtde_parser.cpp @@ -271,19 +271,9 @@ TEST(rtde_parser, test_deprecated_parse_method) std::vector> products; { comm::BinParser bp(raw_data, sizeof(raw_data)); -#ifdef __GNUC__ -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#elif defined(_MSC_VER) -# pragma warning(push) -# pragma warning(disable : 4996) -#endif + URCL_SILENCE_DEPRECATED_BEGIN parser.parse(bp, products); -#ifdef __GNUC__ -# pragma GCC diagnostic pop -#elif defined(_MSC_VER) -# pragma warning(pop) -#endif + URCL_SILENCE_DEPRECATED_END } EXPECT_EQ(products.size(), 1); diff --git a/tests/test_tcp_socket.cpp b/tests/test_tcp_socket.cpp index d64f12ea5..bb6bd317a 100644 --- a/tests/test_tcp_socket.cpp +++ b/tests/test_tcp_socket.cpp @@ -34,13 +34,6 @@ #include #include "test_utils.h" -// This file adds a test for a deprecated function. To avoid a compiler warning in CI (where we want -// to treat warnings as errors) we suppress the warning inside this file. -#ifdef _MSC_VER -# pragma warning(disable : 4996) -#else -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#endif #include #include #include "ur_client_library/types.h" @@ -302,7 +295,9 @@ TEST_F(TCPSocketTest, connect_non_running_robot) TEST_F(TCPSocketTest, test_deprecated_reconnection_time_interface) { + URCL_SILENCE_DEPRECATED_BEGIN client_->setReconnectionTime(std::chrono::milliseconds(100)); + URCL_SILENCE_DEPRECATED_END EXPECT_TRUE(client_->setup(2)); } diff --git a/tests/test_ur_driver.cpp b/tests/test_ur_driver.cpp index d1398c744..f6e299134 100644 --- a/tests/test_ur_driver.cpp +++ b/tests/test_ur_driver.cpp @@ -125,10 +125,9 @@ class UrDriverTest : public ::testing::Test TEST_F(UrDriverTest, read_non_existing_script_file) { const std::string non_existing_script_file = ""; -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + URCL_SILENCE_DEPRECATED_BEGIN EXPECT_THROW(UrDriver::readScriptFile(non_existing_script_file), UrException); -#pragma GCC diagnostic pop + URCL_SILENCE_DEPRECATED_END } TEST_F(UrDriverTest, read_existing_script_file) @@ -145,11 +144,9 @@ TEST_F(UrDriverTest, read_existing_script_file) std::cout << "Failed to create temporary files" << std::endl; GTEST_FAIL(); } -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + URCL_SILENCE_DEPRECATED_BEGIN EXPECT_NO_THROW(UrDriver::readScriptFile(existing_script_file)); -#pragma GCC diagnostic pop - + URCL_SILENCE_DEPRECATED_END // clean up ofs.close(); std::remove(existing_script_file); From c044b4cb1c917c4e817f331e7c082b0709e86bfd Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 4 May 2026 21:23:40 +0200 Subject: [PATCH 26/33] Delete default constructor for Q It should always be constructed with 6 doubles --- include/ur_client_library/types.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/ur_client_library/types.h b/include/ur_client_library/types.h index bfc92205a..116010326 100644 --- a/include/ur_client_library/types.h +++ b/include/ur_client_library/types.h @@ -27,7 +27,6 @@ #include #include #include -#include "ur_client_library/log.h" namespace urcl { @@ -49,6 +48,7 @@ using vector6uint32_t = std::array; */ struct Q { + Q() = delete; Q(const double q1, const double q2, const double q3, const double q4, const double q5, const double q6) { values = { q1, q2, q3, q4, q5, q6 }; From d766304d2e8046063fbefde5e777ce99e9d78efa Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 4 May 2026 21:23:52 +0200 Subject: [PATCH 27/33] More unequality tests for pose --- tests/test_types.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/tests/test_types.cpp b/tests/test_types.cpp index 8d2217e59..120b0ba17 100644 --- a/tests/test_types.cpp +++ b/tests/test_types.cpp @@ -92,6 +92,7 @@ TEST(TestTypes, Pose_equality) const Pose a(1.0, 2.0, 3.0, 0.1, 0.2, 0.3); const Pose b(1.0, 2.0, 3.0, 0.1, 0.2, 0.3); const Pose c(1.1, 2.0, 3.0, 0.1, 0.2, 0.3); + const Pose d(1.1, 2.1, 3.1, 5.1, 5.2, 5.3); const Q q{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; const Pose with_q(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, q); @@ -99,6 +100,7 @@ TEST(TestTypes, Pose_equality) EXPECT_TRUE(a == b); EXPECT_FALSE(a == c); + EXPECT_FALSE(a == d); EXPECT_FALSE(with_q == without_q_near); const Pose with_same_q(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, q); From 450b3f6e33c238416e47cdcf43e1f7fa40fbdb57 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 4 May 2026 21:29:34 +0200 Subject: [PATCH 28/33] Add missing include --- include/ur_client_library/control/motion_primitives.h | 1 + 1 file changed, 1 insertion(+) diff --git a/include/ur_client_library/control/motion_primitives.h b/include/ur_client_library/control/motion_primitives.h index aa5d46caa..f9e451764 100644 --- a/include/ur_client_library/control/motion_primitives.h +++ b/include/ur_client_library/control/motion_primitives.h @@ -32,6 +32,7 @@ #define UR_CLIENT_LIBRARY_MOTION_PRIMITIVES_H_INCLUDED #include +#include #include #include #include From fadbd9a360dc4db773a03f34dae29e1223e156a2 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 5 May 2026 11:30:40 +0200 Subject: [PATCH 29/33] Apply suggestions from code review MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Rune Søe-Knudsen <41109954+urrsk@users.noreply.github.com> --- resources/external_control.urscript | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/resources/external_control.urscript b/resources/external_control.urscript index e484dbbc4..3ecb0c791 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -582,11 +582,11 @@ thread trajectoryThread(): target_pose = p[q[0], q[1], q[2], q[3], q[4], q[5]] target_q = get_target_joint_positions() if has_qnear: - target_q = get_inverse_kin(target_pose, second_block) + local target_q = get_inverse_kin(target_pose, second_block) + movej(target_q, a = acceleration, v = velocity, t = tmptime, r = blend_radius) else: - target_q = get_inverse_kin(target_pose, get_target_joint_positions()) + movej(target_pose, a = acceleration, v = velocity, t = tmptime, r = blend_radius) end - movej(target_q, a = acceleration, v = velocity, t = tmptime, r = blend_radius) # reset old acceleration From 55c1ca4e036c6c2aa43ecc5291c1d245711d4132 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 5 May 2026 11:33:50 +0200 Subject: [PATCH 30/33] Refactor script code --- resources/external_control.urscript | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 3ecb0c791..dc2c6fe69 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -678,23 +678,25 @@ thread trajectoryThread(): if raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEJ: optimovej(q, a = acceleration, v = velocity, r = blend_radius) else: + target_pose = p[q[0], q[1], q[2], q[3], q[4], q[5]] if has_qnear: - target_q = get_inverse_kin(p[q[0], q[1], q[2], q[3], q[4], q[5]], second_block) + local target_q = get_inverse_kin(target_pose, second_block) + optimovej(target_q, a = acceleration, v = velocity, r = blend_radius) else: - target_q = get_inverse_kin(p[q[0], q[1], q[2], q[3], q[4], q[5]], get_target_joint_positions()) + optimovej(target_pose, a = acceleration, v = velocity, r = blend_radius) end - optimovej(target_q, a = acceleration, v = velocity, r = blend_radius) end {% elif ROBOT_SOFTWARE_VERSION >= v10.8.0 %} if raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEJ: optimovej(q, a = acceleration, v = velocity, r = blend_radius) else: + target_pose = p[q[0], q[1], q[2], q[3], q[4], q[5]] if has_qnear: - target_q = get_inverse_kin(p[q[0], q[1], q[2], q[3], q[4], q[5]], second_block) + local target_q = get_inverse_kin(target_pose, second_block) + optimovej(target_q, a = acceleration, v = velocity, r = blend_radius) else: - target_q = get_inverse_kin(p[q[0], q[1], q[2], q[3], q[4], q[5]], get_target_joint_positions()) + optimovej(target_pose, a = acceleration, v = velocity, r = blend_radius) end - optimovej(target_q, a = acceleration, v = velocity, r = blend_radius) end {% else %} popup("Optimovej is only supported from software 10.8.0 and upwards.", error=True, blocking=False) From 79ca4e9a893dbf7fb0804a827bdb62f92b5bd9ca Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 5 May 2026 13:20:04 +0200 Subject: [PATCH 31/33] Make Q and Pose a class --- CMakeLists.txt | 1 + examples/instruction_executor.cpp | 16 +-- include/ur_client_library/types.h | 87 +++++++-------- src/control/motion_primitives.cpp | 6 +- src/control/trajectory_point_interface.cpp | 11 +- src/types.cpp | 118 +++++++++++++++++++++ tests/test_trajectory_point_interface.cpp | 34 +++--- tests/test_types.cpp | 12 +-- 8 files changed, 198 insertions(+), 87 deletions(-) create mode 100644 src/types.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 1226dfc30..17495a7f7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -17,6 +17,7 @@ endif() add_library(urcl + src/types.cpp src/comm/tcp_socket.cpp src/comm/tcp_server.cpp src/control/motion_primitives.cpp diff --git a/examples/instruction_executor.cpp b/examples/instruction_executor.cpp index 2d408aaae..75dcd6d52 100644 --- a/examples/instruction_executor.cpp +++ b/examples/instruction_executor.cpp @@ -111,8 +111,12 @@ int main(int argc, char* argv[]) // To provide a q_near hint for the IK solver a joint configuration near the target can be added to a pose. urcl::Pose target_pose{ -0.206, -0.6437, 0.202, 0.0, 3.140, 0.0 }; - target_pose.q_near = urcl::Q{ -1.7, -4, 1.5, -1, 1.5, 0 }; - instruction_executor->moveJ(target_pose); + target_pose.setQNear(urcl::Q{ -1.7, -4, 1.5, -1, 1.5, 0 }); + instruction_executor->moveJ(target_pose, 2.0, 2.0); + + // q_near can also be set directly upon pose construction + instruction_executor->moveJ( + urcl::Pose{ -0.206, -0.6437, 0.202, 0.0, 3.140, 0.0, urcl::Q{ -1.7, -4, 1.5, -1, 1.5, 0 } }, 2.0, 2.0); // goal time parametrization -- acceleration and velocity will be scaled to meed the goal time. instruction_executor->moveJ({ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 }, 0.1, 0.1, goal_time_sec); @@ -128,14 +132,14 @@ int main(int argc, char* argv[]) instruction_executor->moveJ(urcl::Pose{ -0.0203, 0.363, 0.559, 0.68, -1.083, -2.076 }, 0.1, 0.1, goal_time_sec); // moveP can also be called with brace-init (interpreted as pose) or explicitly using a Pose or Q - instruction_executor->moveP({ -0.2, 0.363, 0.559, 0.68, -1.083, -2.076 }); - instruction_executor->moveP(urcl::Pose{ -0.0203, 0.303, 0.559, 0.68, -1.083, -2.076 }); - instruction_executor->moveP(urcl::Q{ -1.57, -1.83, 1.707, -0.833, 0.782, 0.479 }); + instruction_executor->moveP({ -0.2, 0.363, 0.559, 0.68, -1.083, -2.076 }, 0.2, 0.2); + instruction_executor->moveP(urcl::Pose{ -0.0203, 0.303, 0.559, 0.68, -1.083, -2.076 }, 0.2, 0.2); + instruction_executor->moveP(urcl::Q{ -1.57, -1.83, 1.707, -0.833, 0.782, 0.479 }, 0.2, 0.2); // For moveC via and target can be a Pose or Q. When brace-init style lists are given, values are // interpreted as Pose. instruction_executor->moveC(urcl::Pose{ -0.1, 0.463, 0.559, 0.68, -1.083, -2.076 }, - urcl::Pose{ -0.0203, 0.303, 0.559, 0.68, -1.083, -2.076 }); + urcl::Pose{ -0.3203, 0.303, 0.559, 0.68, -1.083, -2.076 }); instruction_executor->moveC(urcl::Pose{ -0.1, 0.463, 0.559, 0.68, -1.083, -2.076 }, urcl::Q{ -1.57, -1.83, 1.707, -0.833, 0.782, 0.479 }); diff --git a/include/ur_client_library/types.h b/include/ur_client_library/types.h index 116010326..2e6ae572a 100644 --- a/include/ur_client_library/types.h +++ b/include/ur_client_library/types.h @@ -1,4 +1,6 @@ /* + * Copyright 2026, Universal Robots A/S (Adding Q and Pose) + * * Copyright 2019, FZI Forschungszentrum Informatik (refactor) * * Copyright 2017, 2018 Simon Rasmussen (refactor) @@ -21,12 +23,12 @@ #pragma once #include -#include #include #include #include #include #include +#include namespace urcl { @@ -46,41 +48,45 @@ using vector6uint32_t = std::array; * Unlike raw initializer lists (``{...}``) which may bind to either \ref vector6d_t or * \ref Pose, wrapping values in ``urcl::Q{...}`` always forces a joint-target interpretation. */ -struct Q +class Q { +public: Q() = delete; - Q(const double q1, const double q2, const double q3, const double q4, const double q5, const double q6) - { - values = { q1, q2, q3, q4, q5, q6 }; - } - explicit Q(const vector6d_t& values) - { - this->values.resize(6); - std::copy(values.begin(), values.end(), this->values.begin()); - } + Q(const double q1, const double q2, const double q3, const double q4, const double q5, const double q6); + explicit Q(const vector6d_t& values); + + const std::vector& getValues() const; + void setValues(const vector6d_t& values); + void setValues(const std::vector& values); - std::vector values; +private: + std::vector values_; }; -inline bool operator==(const Q& lhs, const Q& rhs) -{ - return lhs.values.size() == rhs.values.size() && std::equal(lhs.values.begin(), lhs.values.end(), rhs.values.begin()); -} +bool operator==(const Q& lhs, const Q& rhs); -struct Pose +/*! + * \brief A Cartesian pose (position and orientation) for a robot manipulator. + * + * The position is represented by ``x``, ``y``, and ``z`` in meters, and the orientation is + * represented by AngleAxis ``rx``, ``ry``, and ``rz`` in radians. + * + * The optional \ref q_near member can be used to provide a hint to the solver for inverse + * kinematics when this pose is used as a Cartesian motion target over the trajectory interface. + */ +class Pose { - Pose() : x(0.0), y(0.0), z(0.0), rx(0.0), ry(0.0), rz(0.0), q_near(std::nullopt) - { - } - Pose(const double x, const double y, const double z, const double rx, const double ry, const double rz) - : x(x), y(y), z(z), rx(rx), ry(ry), rz(rz), q_near(std::nullopt) - { - } +public: + Pose(); + Pose(const double x, const double y, const double z, const double rx, const double ry, const double rz); Pose(const double x, const double y, const double z, const double rx, const double ry, const double rz, - const Q& q_near) - : x(x), y(y), z(z), rx(rx), ry(ry), rz(rz), q_near(q_near) - { - } + const Q& q_near); + + bool operator==(const Pose& other) const; + + const std::optional& getQNear() const; + void setQNear(const Q& q_near); + double x; double y; double z; @@ -88,34 +94,15 @@ struct Pose double ry; double rz; +private: /*! * Optional joint-space hint (six joint positions in radians) passed to the controller for inverse * kinematics when this pose is used as a Cartesian motion target over the trajectory interface. */ - std::optional q_near; - - bool operator==(const Pose& other) const - { - if (x != other.x || y != other.y || z != other.z || rx != other.rx || ry != other.ry || rz != other.rz) - { - return false; - } - if (q_near.has_value() != other.q_near.has_value()) - { - return false; - } - if (!q_near.has_value()) - { - return true; - } - return *q_near == *other.q_near; - } + std::optional q_near_; }; -inline bool operator==(const Q& lhs, const vector6d_t& rhs) -{ - return lhs.values.size() == rhs.size() && std::equal(lhs.values.begin(), lhs.values.end(), rhs.begin()); -} +bool operator==(const Q& lhs, const vector6d_t& rhs); /*! * \brief A tagged union representing either a joint target (\ref Q) or a Cartesian target diff --git a/src/control/motion_primitives.cpp b/src/control/motion_primitives.cpp index 6fe8bfbe3..1862dafb6 100644 --- a/src/control/motion_primitives.cpp +++ b/src/control/motion_primitives.cpp @@ -169,7 +169,8 @@ void MoveJPrimitive::setTarget(const MotionTarget& target) else if constexpr (std::is_same_v) { type = MotionType::MOVEJ; - std::copy(target_variant.values.begin(), target_variant.values.end(), target_joint_configuration.begin()); + const auto& values = target_variant.getValues(); + std::copy(values.begin(), values.end(), target_joint_configuration.begin()); } }, target); @@ -382,7 +383,8 @@ void OptimoveJPrimitive::setTarget(const MotionTarget& target) else if constexpr (std::is_same_v) { type = MotionType::OPTIMOVEJ; - std::copy(target_variant.values.begin(), target_variant.values.end(), target_joint_configuration.begin()); + const auto& values = target_variant.getValues(); + std::copy(values.begin(), values.end(), target_joint_configuration.begin()); } }, target); diff --git a/src/control/trajectory_point_interface.cpp b/src/control/trajectory_point_interface.cpp index efec8b2d2..83ca2c8cb 100644 --- a/src/control/trajectory_point_interface.cpp +++ b/src/control/trajectory_point_interface.cpp @@ -54,8 +54,8 @@ vector6d_t motionTargetToBlock(const urcl::MotionTarget& target) else { static_assert(std::is_same_v, "Unhandled MotionTarget alternative"); - return { alternative.values[0], alternative.values[1], alternative.values[2], - alternative.values[3], alternative.values[4], alternative.values[5] }; + const auto& values = alternative.getValues(); + return { values[0], values[1], values[2], values[3], values[4], values[5] }; } }, target); @@ -129,12 +129,11 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptr; if constexpr (std::is_same_v) { - if (alternative.q_near.has_value()) + if (alternative.getQNear().has_value()) { has_qnear = true; - return vector6d_t{ alternative.q_near->values[0], alternative.q_near->values[1], - alternative.q_near->values[2], alternative.q_near->values[3], - alternative.q_near->values[4], alternative.q_near->values[5] }; + const auto& values = alternative.getQNear()->getValues(); + return vector6d_t{ values[0], values[1], values[2], values[3], values[4], values[5] }; } else { diff --git a/src/types.cpp b/src/types.cpp new file mode 100644 index 000000000..fb0aedde7 --- /dev/null +++ b/src/types.cpp @@ -0,0 +1,118 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2026 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 +{ +Q::Q(const double q1, const double q2, const double q3, const double q4, const double q5, const double q6) +{ + values_ = { q1, q2, q3, q4, q5, q6 }; +} + +Q::Q(const vector6d_t& values) +{ + values_.resize(6); + std::copy(values.begin(), values.end(), values_.begin()); +} + +const std::vector& Q::getValues() const +{ + return values_; +} + +void Q::setValues(const vector6d_t& values) +{ + values_.resize(6); + std::copy(values.begin(), values.end(), values_.begin()); +} + +void Q::setValues(const std::vector& values) +{ + values_ = values; +} + +bool operator==(const Q& lhs, const Q& rhs) +{ + return lhs.getValues().size() == rhs.getValues().size() && + std::equal(lhs.getValues().begin(), lhs.getValues().end(), rhs.getValues().begin()); +} + +Pose::Pose() : x(0.0), y(0.0), z(0.0), rx(0.0), ry(0.0), rz(0.0), q_near_(std::nullopt) +{ +} + +Pose::Pose(const double x, const double y, const double z, const double rx, const double ry, const double rz) + : x(x), y(y), z(z), rx(rx), ry(ry), rz(rz), q_near_(std::nullopt) +{ +} + +Pose::Pose(const double x, const double y, const double z, const double rx, const double ry, const double rz, + const Q& q_near) + : x(x), y(y), z(z), rx(rx), ry(ry), rz(rz), q_near_(q_near) +{ +} + +bool Pose::operator==(const Pose& other) const +{ + if (x != other.x || y != other.y || z != other.z || rx != other.rx || ry != other.ry || rz != other.rz) + { + return false; + } + if (q_near_.has_value() != other.q_near_.has_value()) + { + return false; + } + if (!q_near_.has_value()) + { + return true; + } + return *q_near_ == *other.q_near_; +} + +const std::optional& Pose::getQNear() const +{ + return q_near_; +} + +void Pose::setQNear(const Q& q_near) +{ + q_near_ = q_near; +} + +bool operator==(const Q& lhs, const vector6d_t& rhs) +{ + return lhs.getValues().size() == rhs.size() && + std::equal(lhs.getValues().begin(), lhs.getValues().end(), rhs.begin()); +} + +} // namespace urcl diff --git a/tests/test_trajectory_point_interface.cpp b/tests/test_trajectory_point_interface.cpp index e79efa37c..5e0356c96 100644 --- a/tests/test_trajectory_point_interface.cpp +++ b/tests/test_trajectory_point_interface.cpp @@ -239,8 +239,8 @@ class TrajectoryPointInterfaceTest : public ::testing::Test urcl::Pose pose = toPose(pose_raw); if (toDouble(third_raw[2]) != 0.0) { - pose.q_near = urcl::Q{ toDouble(second_raw[0]), toDouble(second_raw[1]), toDouble(second_raw[2]), - toDouble(second_raw[3]), toDouble(second_raw[4]), toDouble(second_raw[5]) }; + pose.setQNear(urcl::Q{ toDouble(second_raw[0]), toDouble(second_raw[1]), toDouble(second_raw[2]), + toDouble(second_raw[3]), toDouble(second_raw[4]), toDouble(second_raw[5]) }); } return pose; } @@ -768,13 +768,13 @@ TEST_F(TrajectoryPointInterfaceTest, send_movej_pose) TEST_F(TrajectoryPointInterfaceTest, send_movej_pose_with_q_near_roundtrip) { urcl::Pose expected_pose(0.1, -0.2, 0.3, 0.4, -0.5, 0.6); - expected_pose.q_near = urcl::Q{ -1.0, -2.0, 1.0, 0.5, 0.25, 0.0 }; + expected_pose.setQNear(urcl::Q{ -1.0, -2.0, 1.0, 0.5, 0.25, 0.0 }); double blend_radius = 0.25; double velocity = 0.6; double acceleration = 0.7; auto duration = std::chrono::milliseconds(500); urcl::MotionTarget target{ std::in_place_type, 0.1, -0.2, 0.3, 0.4, -0.5, 0.6 }; - std::get(target).q_near = urcl::Q{ -1.0, -2.0, 1.0, 0.5, 0.25, 0.0 }; + std::get(target).setQNear(urcl::Q{ -1.0, -2.0, 1.0, 0.5, 0.25, 0.0 }); auto primitive = std::make_shared(std::move(target), blend_radius, duration, acceleration, velocity); @@ -789,7 +789,7 @@ TEST_F(TrajectoryPointInterfaceTest, send_movej_pose_with_q_near_roundtrip) TEST_F(TrajectoryPointInterfaceTest, send_movel_with_q_near_roundtrip) { urcl::Pose send_positions(1.2, 3.1, 2.2, -3.4, -1.1, -1.2); - send_positions.q_near = urcl::Q{ 0.11, -0.22, 0.33, 0.44, -0.55, 0.66 }; + send_positions.setQNear(urcl::Q{ 0.11, -0.22, 0.33, 0.44, -0.55, 0.66 }); double blend_radius = 0.5; double velocity = 0.6; double acceleration = 0.7; @@ -812,7 +812,7 @@ TEST_F(TrajectoryPointInterfaceTest, send_optimovej_pose_with_q_near_roundtrip) double acceleration_fraction = 0.4; double velocity_fraction = 0.6; urcl::MotionTarget target{ std::in_place_type, 0.1, -0.2, 0.3, 0.4, -0.5, 0.6 }; - std::get(target).q_near = urcl::Q{ 0.01, 0.02, -0.03, 0.04, -0.05, 0.06 }; + std::get(target).setQNear(urcl::Q{ 0.01, 0.02, -0.03, 0.04, -0.05, 0.06 }); auto primitive = std::make_shared(std::move(target), blend_radius, acceleration_fraction, velocity_fraction); @@ -820,7 +820,7 @@ TEST_F(TrajectoryPointInterfaceTest, send_optimovej_pose_with_q_near_roundtrip) auto received_primitive = client_->getMotionPrimitive(); EXPECT_EQ(received_primitive->type, control::MotionType::OPTIMOVEJ_POSE); urcl::Pose expected_with_q = expected_pose; - expected_with_q.q_near = urcl::Q{ 0.01, 0.02, -0.03, 0.04, -0.05, 0.06 }; + expected_with_q.setQNear(urcl::Q{ 0.01, 0.02, -0.03, 0.04, -0.05, 0.06 }); EXPECT_EQ(std::get( std::static_pointer_cast(received_primitive)->getTarget().value()), expected_with_q); @@ -841,8 +841,8 @@ TEST_F(TrajectoryPointInterfaceTest, send_movel_joint) EXPECT_EQ(received_primitive->type, control::MotionType::MOVEL_JOINT); EXPECT_EQ(std::get( std::static_pointer_cast(received_primitive)->getTarget().value()) - .values, - send_joints.values); + .getValues(), + send_joints.getValues()); EXPECT_EQ(received_primitive->blend_radius, blend_radius); EXPECT_EQ(received_primitive->velocity, velocity); EXPECT_EQ(received_primitive->acceleration, acceleration); @@ -863,8 +863,8 @@ TEST_F(TrajectoryPointInterfaceTest, send_movep_joint) EXPECT_EQ(received_primitive->type, control::MotionType::MOVEP_JOINT); EXPECT_EQ(std::get( std::static_pointer_cast(received_primitive)->getTarget().value()) - .values, - send_joints.values); + .getValues(), + send_joints.getValues()); EXPECT_EQ(received_primitive->blend_radius, blend_radius); EXPECT_EQ(received_primitive->velocity, velocity); EXPECT_EQ(received_primitive->acceleration, acceleration); @@ -885,8 +885,8 @@ TEST_F(TrajectoryPointInterfaceTest, send_movec_joint_joint) auto received_primitive = client_->getMotionPrimitive(); EXPECT_EQ(received_primitive->type, control::MotionType::MOVEC_JOINT); auto movec = std::static_pointer_cast(received_primitive); - EXPECT_EQ(std::get(movec->getTarget().value()).values, send_target.values); - EXPECT_EQ(std::get(movec->getVia()).values, send_via.values); + EXPECT_EQ(std::get(movec->getTarget().value()).getValues(), send_target.getValues()); + EXPECT_EQ(std::get(movec->getVia()).getValues(), send_via.getValues()); EXPECT_EQ(received_primitive->blend_radius, blend_radius); EXPECT_EQ(received_primitive->acceleration, acceleration); EXPECT_EQ(received_primitive->velocity, velocity); @@ -911,7 +911,7 @@ TEST_F(TrajectoryPointInterfaceTest, send_movec_pose_joint) EXPECT_EQ(received_primitive->type, control::MotionType::MOVEC_JOINT_POSE); auto movec = std::static_pointer_cast(received_primitive); EXPECT_EQ(std::get(movec->getVia()), send_via); - EXPECT_EQ(std::get(movec->getTarget().value()).values, send_target.values); + EXPECT_EQ(std::get(movec->getTarget().value()).getValues(), send_target.getValues()); EXPECT_EQ(received_primitive->blend_radius, blend_radius); EXPECT_EQ(received_primitive->acceleration, acceleration); EXPECT_EQ(received_primitive->velocity, velocity); @@ -935,7 +935,7 @@ TEST_F(TrajectoryPointInterfaceTest, send_movec_joint_pose) // via is a joint configuration, target is a pose -> MOVEC_POSE_JOINT EXPECT_EQ(received_primitive->type, control::MotionType::MOVEC_POSE_JOINT); auto movec = std::static_pointer_cast(received_primitive); - EXPECT_EQ(std::get(movec->getVia()).values, send_via.values); + EXPECT_EQ(std::get(movec->getVia()).getValues(), send_via.getValues()); EXPECT_EQ(std::get(movec->getTarget().value()), send_target); EXPECT_EQ(received_primitive->blend_radius, blend_radius); EXPECT_EQ(received_primitive->acceleration, acceleration); @@ -978,8 +978,8 @@ TEST_F(TrajectoryPointInterfaceTest, send_optimovel_joint) EXPECT_EQ(received_primitive->type, control::MotionType::OPTIMOVEL_JOINT); EXPECT_EQ(std::get( std::static_pointer_cast(received_primitive)->getTarget().value()) - .values, - send_joints.values); + .getValues(), + send_joints.getValues()); EXPECT_EQ(received_primitive->blend_radius, blend_radius); EXPECT_EQ(received_primitive->velocity, velocity_fraction); EXPECT_EQ(received_primitive->acceleration, acceleration_fraction); diff --git a/tests/test_types.cpp b/tests/test_types.cpp index 120b0ba17..ac2504f53 100644 --- a/tests/test_types.cpp +++ b/tests/test_types.cpp @@ -44,8 +44,8 @@ TEST(TestTypes, Q_constructors_and_equality) const Q from_array(arr); EXPECT_EQ(from_doubles, from_array); - EXPECT_EQ(from_doubles.values.size(), 6u); - EXPECT_EQ(from_array.values.size(), 6u); + EXPECT_EQ(from_doubles.getValues().size(), 6u); + EXPECT_EQ(from_array.getValues().size(), 6u); const Q different{ 1.0, 0.2, 0.3, 0.4, 0.5, 0.6 }; EXPECT_FALSE(from_doubles == different); @@ -70,7 +70,7 @@ TEST(TestTypes, Pose_default_and_constructors) EXPECT_DOUBLE_EQ(default_pose.rx, 0.0); EXPECT_DOUBLE_EQ(default_pose.ry, 0.0); EXPECT_DOUBLE_EQ(default_pose.rz, 0.0); - EXPECT_FALSE(default_pose.q_near.has_value()); + EXPECT_FALSE(default_pose.getQNear().has_value()); const Pose cartesian(1.0, 2.0, 3.0, 0.1, 0.2, 0.3); EXPECT_DOUBLE_EQ(cartesian.x, 1.0); @@ -79,12 +79,12 @@ TEST(TestTypes, Pose_default_and_constructors) EXPECT_DOUBLE_EQ(cartesian.rx, 0.1); EXPECT_DOUBLE_EQ(cartesian.ry, 0.2); EXPECT_DOUBLE_EQ(cartesian.rz, 0.3); - EXPECT_FALSE(cartesian.q_near.has_value()); + EXPECT_FALSE(cartesian.getQNear().has_value()); const Q hint{ 0.0, -0.5, 1.0, 0.0, 0.5, 0.0 }; const Pose with_hint(10.0, 20.0, 30.0, 1.0, 2.0, 3.0, hint); - ASSERT_TRUE(with_hint.q_near.has_value()); - EXPECT_EQ(*with_hint.q_near, hint); + ASSERT_TRUE(with_hint.getQNear().has_value()); + EXPECT_EQ(*with_hint.getQNear(), hint); } TEST(TestTypes, Pose_equality) From af4f7f0f15305cdb2be3d5051304a82b4c0d36b6 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 6 May 2026 09:33:19 +0200 Subject: [PATCH 32/33] Add setPose and tests for setting Q and Pose values --- include/ur_client_library/types.h | 2 ++ src/types.cpp | 14 ++++++++++++++ tests/test_types.cpp | 27 +++++++++++++++++++++++++++ 3 files changed, 43 insertions(+) diff --git a/include/ur_client_library/types.h b/include/ur_client_library/types.h index 2e6ae572a..f50ab3e4e 100644 --- a/include/ur_client_library/types.h +++ b/include/ur_client_library/types.h @@ -87,6 +87,8 @@ class Pose const std::optional& getQNear() const; void setQNear(const Q& q_near); + void setPose(const double x, const double y, const double z, const double rx, const double ry, const double rz); + double x; double y; double z; diff --git a/src/types.cpp b/src/types.cpp index fb0aedde7..5237735a5 100644 --- a/src/types.cpp +++ b/src/types.cpp @@ -58,6 +58,10 @@ void Q::setValues(const vector6d_t& values) void Q::setValues(const std::vector& values) { + if (values.size() != 6) + { + throw std::invalid_argument("Q must have exactly 6 values"); + } values_ = values; } @@ -115,4 +119,14 @@ bool operator==(const Q& lhs, const vector6d_t& rhs) std::equal(lhs.getValues().begin(), lhs.getValues().end(), rhs.begin()); } +void Pose::setPose(const double x, const double y, const double z, const double rx, const double ry, const double rz) +{ + this->x = x; + this->y = y; + this->z = z; + this->rx = rx; + this->ry = ry; + this->rz = rz; +} + } // namespace urcl diff --git a/tests/test_types.cpp b/tests/test_types.cpp index ac2504f53..ea1ed7ced 100644 --- a/tests/test_types.cpp +++ b/tests/test_types.cpp @@ -61,6 +61,21 @@ TEST(TestTypes, Q_equals_vector6d) EXPECT_FALSE(q == mismatch); } +TEST(TestTypes, Q_setValues_and_getValues) +{ + Q q{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + + const vector6d_t from_array{ { 0.1, 0.2, 0.3, 0.4, 0.5, 0.6 } }; + q.setValues(from_array); + EXPECT_TRUE(q == from_array); + EXPECT_EQ(q.getValues().size(), 6u); + + const std::vector from_vector{ 1.1, 1.2, 1.3, 1.4, 1.5, 1.6 }; + q.setValues(from_vector); + EXPECT_EQ(q.getValues(), from_vector); + EXPECT_EQ(q.getValues().size(), 6u); +} + TEST(TestTypes, Pose_default_and_constructors) { Pose default_pose; @@ -111,6 +126,18 @@ TEST(TestTypes, Pose_equality) EXPECT_FALSE(with_q == different_hint); } +TEST(TestTypes, set_pose) +{ + Pose p; + p.setPose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3); + EXPECT_DOUBLE_EQ(p.x, 1.0); + EXPECT_DOUBLE_EQ(p.y, 2.0); + EXPECT_DOUBLE_EQ(p.z, 3.0); + EXPECT_DOUBLE_EQ(p.rx, 0.1); + EXPECT_DOUBLE_EQ(p.ry, 0.2); + EXPECT_DOUBLE_EQ(p.rz, 0.3); +} + TEST(TestTypes, MotionTarget_variant) { const MotionTarget joint_side = Q{ 0.0, 0.1, 0.2, 0.3, 0.4, 0.5 }; From 3616b95857a7e49938f40e409fc7e255562607ce Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 6 May 2026 10:34:37 +0200 Subject: [PATCH 33/33] Do not call setVia in MoveC constructors Since the via_point is set in the initializer list already, and setTarget handles all recomputation, we can skip this redundant step. --- src/control/motion_primitives.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/control/motion_primitives.cpp b/src/control/motion_primitives.cpp index 1862dafb6..e6810cf92 100644 --- a/src/control/motion_primitives.cpp +++ b/src/control/motion_primitives.cpp @@ -260,7 +260,6 @@ MoveCPrimitive::MoveCPrimitive(const urcl::Pose& via_point, const urcl::Pose& ta , mode(mode) , via_point_(via_point) { - setVia(via_point); setTarget(target); } @@ -270,7 +269,6 @@ MoveCPrimitive::MoveCPrimitive(const MotionTarget& via_point, const MotionTarget , mode(mode) , via_point_(via_point) { - setVia(via_point); setTarget(target); }