[Instruction Executor], [wire-breaking] Allow joint and pose targets for all motion functions (UniversalRobots#491)

All script functions interfaced through the InstructionExecutor allow
their targets to be either defined as joint array or as poses. This
change makes the same interface available on the c++ Instruction
executor. See the example for different calls.

This effectively breaks the wire protocol for the trajectory point interface. If users have their own external_control script code implementations, they should update them accordingly.

Author: urfeex

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

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<urcl::Q>``), 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.

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``.

doc/examples/instruction_executor.rst

@@ -50,20 +50,41 @@ 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++
    :caption: examples/instruction_executor.cpp
    :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`.

examples/instruction_executor.cpp

@@ -89,6 +89,8 @@ int main(int argc, char* argv[])
 
     std::make_shared<urcl::control::MoveLPrimitive>(urcl::Pose(-0.203, 0.263, 0.559, 0.68, -1.083, -2.076), 0.1,
                                                     std::chrono::seconds(2)),
+    std::make_shared<urcl::control::MoveLPrimitive>(urcl::Q{ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 }, 0.1,
+                                                    std::chrono::seconds(2)),
     std::make_shared<urcl::control::MovePPrimitive>(urcl::Pose{ -0.203, 0.463, 0.559, 0.68, -1.083, -2.076 }, 0.1, 0.4,
                                                     0.4),
     std::make_shared<urcl::control::OptimoveJPrimitive>(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;
-}
+}