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/doc/architecture/instruction_executor.rst b/doc/architecture/instruction_executor.rst index c143d983f..59113bbe4 100644 --- a/doc/architecture/instruction_executor.rst +++ b/doc/architecture/instruction_executor.rst @@ -16,6 +16,26 @@ 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. + +An ``urcl::Pose`` may optionally carry ``q_near`` (``std::optional``), a joint configuration +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` 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 08a0d0990..f16900d84 100644 --- a/doc/architecture/trajectory_point_interface.rst +++ b/doc/architecture/trajectory_point_interface.rst @@ -41,29 +41,71 @@ 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) - - 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``) - - SPLINE: spline type (1: CUBIC, 2: QUINTIC) + 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 20 (see below). + + 6-11 Depending on the motion type, this represents either + + - 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 20). + - trajectory point velocities (multiplied by ``MULT_JOINTSTATE``) for spline joint types + + 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_*``: 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). + + 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. + + - 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 - Cubic or quintic spline. ===== ===== where @@ -75,3 +117,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 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 d9ca495c1..04bc9b71c 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,20 @@ 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``. + +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`. diff --git a/examples/instruction_executor.cpp b/examples/instruction_executor.cpp index 85fa5ade0..75dcd6d52 100644 --- a/examples/instruction_executor.cpp +++ b/examples/instruction_executor.cpp @@ -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::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, @@ -100,17 +102,52 @@ int main(int argc, char* argv[]) double goal_time_sec = 2.0; - // acceleration / velocity parametrization - instruction_executor->moveJ({ -1.57, -1.57, 0, 0, 0, 0 }, 2.0, 2.0); - // goal time parametrization -- acceleration and velocity will be ignored + // acceleration / velocity parametrization brace-init style will be interpreted as joint + // positions + instruction_executor->moveJ({ -1.742, -1.726, -2.214, -0.773, 1.572, -0.171 }, 2.0, 2.0); + + // 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.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); - // 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->moveP({ -0.203, 0.463, 0.759, 0.68, -1.083, -2.076 }, 1.5, 1.5); + // 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); + + // 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.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.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 }); + + // 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; -} \ 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..f9e451764 100644 --- a/include/ur_client_library/control/motion_primitives.h +++ b/include/ur_client_library/control/motion_primitives.h @@ -32,7 +32,9 @@ #define UR_CLIENT_LIBRARY_MOTION_PRIMITIVES_H_INCLUDED #include +#include #include +#include #include namespace urcl @@ -40,18 +42,42 @@ 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 = 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 }; +std::string motionTypeToString(const MotionType type); + /*! * Spline types */ @@ -61,8 +87,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; @@ -73,72 +106,149 @@ 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 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; } - urcl::vector6d_t target_joint_configuration; + virtual void setTarget(const MotionTarget& target) = 0; + + [[nodiscard]] + std::optional getTarget() const + { + if (target_ != nullptr) + { + return *target_; + } + return std::nullopt; + } + +protected: + std::unique_ptr target_; }; -struct MoveLPrimitive : public MotionPrimitive +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. 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); + + void setTarget(const MotionTarget& target) override; + + [[deprecated("Use getTarget() and setTarget() instead.")]] + urcl::vector6d_t target_joint_configuration{}; +}; + +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); - urcl::Pose target_pose; + /*! + * \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. 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); + + ~MoveLPrimitive() override; + + void setTarget(const MotionTarget& target) override; + + [[deprecated("Use getTarget() and setTarget() instead.")]] + urcl::Pose target_pose{}; }; -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. + * + * 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); - urcl::Pose target_pose; + ~MovePPrimitive() override; + + void setTarget(const MotionTarget& target) override; + + [[deprecated("Use getTarget() and setTarget() instead.")]] + urcl::Pose target_pose{}; }; -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) + 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. + * + * 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``). 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. + */ + 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); + + MotionTarget getVia() const { - 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; + return via_point_; } - urcl::Pose via_point_pose; - urcl::Pose target_pose; + [[deprecated("Use getVia() and setVia() instead.")]] + urcl::Pose via_point_pose{}; + [[deprecated("Use getTarget() and setTarget() instead.")]] + urcl::Pose target_pose{}; int32_t mode = 0; + +protected: + urcl::MotionTarget via_point_; + +private: + void recomputeType(); + + void refreshLegacyFields(); }; struct SplinePrimitive : public MotionPrimitive @@ -173,39 +283,52 @@ 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. + * + * 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); + + void setTarget(const MotionTarget& target) override; bool validate() const override; - urcl::vector6d_t target_joint_configuration; + [[deprecated("Use getTarget() and setTarget() instead.")]] + urcl::vector6d_t target_joint_configuration{}; }; -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. + * + * 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); + + ~OptimoveLPrimitive() override; + + void setTarget(const MotionTarget& target) override; bool validate() const override; - urcl::Pose target_pose; + [[deprecated("Use getTarget() and setTarget() instead.")]] + urcl::Pose target_pose{}; }; } // 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/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/include/ur_client_library/types.h b/include/ur_client_library/types.h index 861925772..f50ab3e4e 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) @@ -24,7 +26,9 @@ #include #include #include -#include "ur_client_library/log.h" +#include +#include +#include namespace urcl { @@ -33,15 +37,58 @@ using vector6d_t = std::array; using vector6int32_t = std::array; using vector6uint32_t = std::array; -struct Pose +/*! + * \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. + */ +class Q { - Pose() : x(0.0), y(0.0), z(0.0), rx(0.0), ry(0.0), rz(0.0) - { - } - 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) - { - } +public: + Q() = delete; + 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); + +private: + std::vector values_; +}; + +bool operator==(const Q& lhs, const Q& rhs); + +/*! + * \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 +{ +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); + + bool operator==(const Pose& other) const; + + 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; @@ -49,12 +96,32 @@ struct Pose double ry; double rz; - 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; - } +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 Q& lhs, const vector6d_t& rhs); + +/*! + * \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 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 f5fd28956..8669b5ec1 100644 --- a/include/ur_client_library/ur/instruction_executor.h +++ b/include/ur_client_library/ur/instruction_executor.h @@ -81,6 +81,31 @@ 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); + /** * \brief Move the robot to a pose target using movel * @@ -98,6 +123,29 @@ 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); + /** * \brief Move the robot to a pose target using movep * @@ -114,6 +162,26 @@ 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); + /** * \brief Move the robot to a pose target using movec * @@ -132,6 +200,29 @@ 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); + /** * \brief Move the robot to a joint target using optimoveJ. * @@ -150,6 +241,26 @@ 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); + /** * \brief Move the robot to a pose target using optimoveL. * @@ -168,6 +279,26 @@ 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); + /** * \brief Cancel the current motion. * diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 2462f3517..dc2c6fe69 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -40,6 +40,14 @@ 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_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 @@ -557,42 +565,81 @@ 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 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: + target_pose = p[q[0], q[1], q[2], q[3], q[4], q[5]] + target_q = get_target_joint_positions() + if has_qnear: + local target_q = get_inverse_kin(target_pose, second_block) + movej(target_q, a = acceleration, v = velocity, t = tmptime, r = blend_radius) + else: + movej(target_pose, a = acceleration, v = velocity, t = tmptime, r = blend_radius) + end + + # 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: - 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: + 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 - 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: + movep(q, 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_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 = 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]] + 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 - elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEC: - 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]] - acceleration = raw_point[13] / MULT_jointstate - velocity = raw_point[14] / MULT_jointstate mode = raw_point[15] / MULT_jointstate movec(via, target, acceleration, velocity, blend_radius, mode) @@ -625,14 +672,32 @@ thread trajectoryThread(): end # OptimoveJ point - elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEJ: - acceleration = raw_point[13] / MULT_jointstate - velocity = raw_point[7] / MULT_jointstate + elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEJ or raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEJ_POSE: {% 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: + target_pose = p[q[0], q[1], q[2], q[3], q[4], q[5]] + if has_qnear: + local target_q = get_inverse_kin(target_pose, second_block) + optimovej(target_q, a = acceleration, v = velocity, r = blend_radius) + else: + optimovej(target_pose, a = acceleration, v = velocity, r = blend_radius) + end + 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: + target_pose = p[q[0], q[1], q[2], q[3], q[4], q[5]] + if has_qnear: + local target_q = get_inverse_kin(target_pose, second_block) + optimovej(target_q, a = acceleration, v = velocity, r = blend_radius) + else: + optimovej(target_pose, a = acceleration, v = velocity, r = blend_radius) + end + end {% else %} popup("Optimovej is only supported from software 10.8.0 and upwards.", error=True, blocking=False) {% endif %} @@ -653,15 +718,21 @@ thread trajectoryThread(): spline_qd = [0, 0, 0, 0, 0, 0] # OptimoveL point - elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEL: - acceleration = raw_point[13] / MULT_jointstate - velocity = raw_point[7] / MULT_jointstate - + elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEL or raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEL_JOINT: + {% 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 %} @@ -703,8 +774,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: @@ -715,7 +786,7 @@ thread clearTrajectoryPointsThread(): trajectory_points_left = trajectory_points_left - 1 reads = reads + 1 if reads >= limit: - sync() + sync() reads = 0 end end @@ -936,17 +1007,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 @@ -989,7 +1060,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/motion_primitives.cpp b/src/control/motion_primitives.cpp index 89cc72171..e6810cf92 100644 --- a/src/control/motion_primitives.cpp +++ b/src/control/motion_primitives.cpp @@ -28,11 +28,13 @@ // POSSIBILITY OF SUCH DAMAGE. // -- END LICENSE BLOCK ------------------------------------------------ +#include #include #include namespace urcl::control { +URCL_SILENCE_DEPRECATED_BEGIN bool MotionPrimitive::validate() const { @@ -97,4 +99,333 @@ 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"; + } +} + +MoveJPrimitive::MoveJPrimitive(const MotionTarget& target, const double blend_radius, + const std::chrono::duration duration, const double acceleration, + const double 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(blend_radius, duration, acceleration, velocity) +{ + setTarget(Q(target)); +} + +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; + const auto& values = target_variant.getValues(); + std::copy(values.begin(), 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(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(blend_radius, duration, acceleration, velocity) +{ + setTarget(target); +} + +MoveLPrimitive::~MoveLPrimitive() +{ +} + +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(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(blend_radius, std::chrono::milliseconds(0), acceleration, velocity) +{ + setTarget(target); +} + +MovePPrimitive::~MovePPrimitive() +{ +} + +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(blend_radius, std::chrono::milliseconds(0), acceleration, velocity) + , mode(mode) + , via_point_(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(blend_radius, std::chrono::milliseconds(0), acceleration, velocity) + , mode(mode) + , via_point_(via_point) +{ + setTarget(target); +} + +MoveCPrimitive::~MoveCPrimitive() +{ +} + +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() +{ + 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) + { + 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() +{ + 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(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(blend_radius, std::chrono::milliseconds(0), acceleration_fraction, velocity_fraction) +{ + setTarget(target); +} + +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; + const auto& values = target_variant.getValues(); + std::copy(values.begin(), 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(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(blend_radius, std::chrono::milliseconds(0), acceleration_fraction, velocity_fraction) +{ + setTarget(target); +} + +OptimoveLPrimitive::~OptimoveLPrimitive() +{ +} + +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); +} + +URCL_SILENCE_DEPRECATED_END + +} // namespace urcl::control diff --git a/src/control/trajectory_point_interface.cpp b/src/control/trajectory_point_interface.cpp index 0e72d5209..83ca2c8cb 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"); + const auto& values = alternative.getValues(); + return { values[0], values[1], values[2], values[3], values[4], values[5] }; + } + }, + target); +} +} // namespace std::string trajectoryResultToString(const TrajectoryResult result) { @@ -89,45 +111,54 @@ 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: 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: 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); - 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 - }; - second_block.fill(primitive->velocity); - third_block.fill(primitive->acceleration); + auto with_target = std::static_pointer_cast(primitive); + first_block = motionTargetToBlock(with_target->getTarget().value()); + 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.getQNear().has_value()) + { + has_qnear = true; + const auto& values = alternative.getQNear()->getValues(); + return vector6d_t{ values[0], values[1], values[2], values[3], values[4], 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: + 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 }; + 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 }; @@ -144,24 +175,6 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptr(primitive); - first_block = optimovej_primitive->target_joint_configuration; - second_block.fill(primitive->velocity); - third_block.fill(primitive->acceleration); - break; - } - case control::MotionType::OPTIMOVEL: - { - auto optimovel_primitive = std::static_pointer_cast(primitive); - first_block = { optimovel_primitive->target_pose.x, optimovel_primitive->target_pose.y, - optimovel_primitive->target_pose.z, optimovel_primitive->target_pose.rx, - optimovel_primitive->target_pose.ry, optimovel_primitive->target_pose.rz }; - second_block.fill(primitive->velocity); - third_block.fill(primitive->acceleration); - break; - } default: throw UnsupportedMotionType(); } diff --git a/src/types.cpp b/src/types.cpp new file mode 100644 index 000000000..5237735a5 --- /dev/null +++ b/src/types.cpp @@ -0,0 +1,132 @@ +// -- 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) +{ + if (values.size() != 6) + { + throw std::invalid_argument("Q must have exactly 6 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()); +} + +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/src/ur/instruction_executor.cpp b/src/ur/instruction_executor.cpp index ab6243e31..da2d11180 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"); + 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. } @@ -99,6 +99,12 @@ 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(Q(target), acceleration, velocity, time, 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( target, blend_radius, std::chrono::milliseconds(static_cast(time * 1000)), acceleration, velocity) }); @@ -106,6 +112,12 @@ bool urcl::InstructionExecutor::moveJ(const urcl::vector6d_t& target, const doub bool urcl::InstructionExecutor::moveL(const urcl::Pose& target, const double acceleration, const double velocity, const double time, const double blend_radius) +{ + 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) { return executeMotion({ std::make_shared( target, blend_radius, std::chrono::milliseconds(static_cast(time * 1000)), acceleration, velocity) }); @@ -113,12 +125,24 @@ bool urcl::InstructionExecutor::moveL(const urcl::Pose& target, const double acc bool urcl::InstructionExecutor::moveP(const urcl::Pose& target, const double acceleration, const double velocity, const double blend_radius) +{ + 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) { 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) +{ + return moveC(MotionTarget(via), MotionTarget(target), acceleration, velocity, blend_radius, 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) }); @@ -126,12 +150,24 @@ bool urcl::InstructionExecutor::moveC(const urcl::Pose& via, const urcl::Pose& t bool urcl::InstructionExecutor::optimoveJ(const urcl::vector6d_t& target, const double acceleration, const double velocity, const double blend_radius) +{ + return optimoveJ(Q(target), acceleration, velocity, blend_radius); +} + +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 optimoveL(MotionTarget(target), acceleration, velocity, blend_radius); +} + +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) }); } 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/CMakeLists.txt b/tests/CMakeLists.txt index b5d32993f..a7abb4909 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -275,3 +275,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_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_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_primary_client.cpp b/tests/test_primary_client.cpp index c24dec2d7..1b99eafa0 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 a9b84e2b6..becdf1325 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_trajectory_point_interface.cpp b/tests/test_trajectory_point_interface.cpp index 1084ce4c4..5e0356c96 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; @@ -214,71 +215,130 @@ 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]) }; + } + + /*! \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.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; + } + std::shared_ptr getMotionPrimitive() { TrajData spl = getData(); + const double blend_radius = toDouble(spl.blend_radius_or_spline_type); + // 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)) + { + return std::make_shared(toVector(spl.pos), blend_radius, duration, acceleration, + velocity); + } + else if (spl.motion_type == static_cast(control::MotionType::MOVEJ_POSE)) { 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); + 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(poseWithOptionalQNear(spl.pos, spl.vel, spl.acc), 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(poseWithOptionalQNear(spl.pos, spl.vel, spl.acc), 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::MOVEC)) + else if (spl.motion_type == static_cast(control::MotionType::OPTIMOVEJ_POSE)) { - 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))); + 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(poseWithOptionalQNear(spl.pos, spl.vel, spl.acc), + blend_radius, acceleration, velocity); + } + else if (spl.motion_type == static_cast(control::MotionType::OPTIMOVEL_JOINT)) + { + 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 { @@ -384,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), @@ -429,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), @@ -480,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 @@ -546,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) @@ -566,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) @@ -615,7 +669,8 @@ 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, + 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); @@ -625,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; @@ -636,7 +691,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().value()), + send_positions); EXPECT_EQ(received_primitive->blend_radius, blend_radius); EXPECT_EQ(received_primitive->velocity, velocity); EXPECT_EQ(received_primitive->acceleration, acceleration); @@ -645,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; @@ -654,7 +711,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().value()), + send_positions); EXPECT_EQ(received_primitive->blend_radius, blend_radius); EXPECT_EQ(received_primitive->velocity, velocity); EXPECT_EQ(received_primitive->acceleration, acceleration); @@ -662,10 +721,10 @@ 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.7; + double acceleration = 0.4; double velocity = 0.7; int32_t mode = 1; auto primitive = @@ -674,12 +733,256 @@ 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().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); - EXPECT_EQ(received_primitive->acceleration, velocity); - EXPECT_EQ(std::static_pointer_cast(received_primitive)->mode, mode); + EXPECT_EQ(received_primitive->velocity, velocity); + EXPECT_EQ(received_movec->mode, mode); +} + +TEST_F(TrajectoryPointInterfaceTest, send_movej_pose) +{ + 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{ 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()), + 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.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).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); + + 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.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; + 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).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); + + 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.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); +} + +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::get( + std::static_pointer_cast(received_primitive)->getTarget().value()) + .getValues(), + send_joints.getValues()); + 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::get( + std::static_pointer_cast(received_primitive)->getTarget().value()) + .getValues(), + send_joints.getValues()); + 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(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); + 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(std::get(movec->getVia()), send_via); + 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); + 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(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); + EXPECT_EQ(received_primitive->velocity, velocity); + EXPECT_EQ(movec->mode, mode); +} + +TEST_F(TrajectoryPointInterfaceTest, send_optimovej_pose) +{ + 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{ 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()), + expected_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::get( + std::static_pointer_cast(received_primitive)->getTarget().value()) + .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); } TEST_F(TrajectoryPointInterfaceTest, unsupported_motion_type_throws) diff --git a/tests/test_types.cpp b/tests/test_types.cpp new file mode 100644 index 000000000..ea1ed7ced --- /dev/null +++ b/tests/test_types.cpp @@ -0,0 +1,194 @@ +// -- 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.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); +} + +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, 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; + 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.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); + 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.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.getQNear().has_value()); + EXPECT_EQ(*with_hint.getQNear(), 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 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); + 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(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); + 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, 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 }; + 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); +} 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);