Implement MotionTargets for all motion types

urfeex · urfeex · commit ebaa2e3595dc · 2026-04-24T11:30:35.000+02:00
diff --git a/examples/instruction_executor.cpp b/examples/instruction_executor.cpp
@@ -98,20 +98,44 @@ int main(int argc, char* argv[])
     std::make_shared<urcl::control::OptimoveLPrimitive>(urcl::Pose(-0.203, 0.263, 0.559, 0.68, -1.083, -2.076), 0.1,
                                                         0.4, 0.7),
   };
-  // instruction_executor->executeMotion(motion_sequence);
+  instruction_executor->executeMotion(motion_sequence);
 
   double goal_time_sec = 2.0;
 
-  // acceleration / velocity parametrization
-  // instruction_executor->moveJ({ -1.57, -1.57, 0, 0, 0, 0 }, 2.0, 2.0);
+  // acceleration / velocity parametrization brace-init style will be interpreted as joint
+  // positions
+  instruction_executor->moveJ({ -1.57, -1.57, 0, 0, 0, 0 }, 2.0, 2.0);
+
   // goal time parametrization -- acceleration and velocity will be ignored
   instruction_executor->moveJ({ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 }, 0.1, 0.1, goal_time_sec);
+
+  // moveL calls with brace-init style is interpreted as a pose
   instruction_executor->moveL({ -0.0203, 0.363, 0.559, 0.68, -1.083, -2.076 }, 1.5, 1.5);
+  // A pose can also be explicitly passed to moveL
+  instruction_executor->moveL(urcl::Pose{ -0.0203, 0.363, 0.559, 0.68, -1.083, -2.076 }, 1.5, 1.5);
+  // moveL can also accept a joint position target, if explicitly wrapped into a urcl::Q object
   instruction_executor->moveL(urcl::Q{ -1.572, -1.686, 1.707, -0.833, 0.782, 0.479 }, 1.5, 1.5, goal_time_sec);
+
+  // moveJ can also accept a Cartesian pose, when given explicitly
   instruction_executor->moveJ(urcl::Pose{ -0.0203, 0.363, 0.559, 0.68, -1.083, -2.076 }, 0.1, 0.1, goal_time_sec);
 
-  // instruction_executor->moveP({ -0.203, 0.463, 0.759, 0.68, -1.083, -2.076 }, 1.5, 1.5);
+  // moveP can also be called with brace-init (interpreted as pose) or explicitly using a Pose or Q
+  instruction_executor->moveP({ -0.2, 0.363, 0.559, 0.68, -1.083, -2.076 }, 0.1, 0.1, goal_time_sec);
+  instruction_executor->moveP(urcl::Pose{ -0.0203, 0.303, 0.559, 0.68, -1.083, -2.076 }, 0.1, 0.1, goal_time_sec);
+  instruction_executor->moveP(urcl::Q{ -1.57, -1.83, 1.707, -0.833, 0.782, 0.479 }, 0.1, 0.1, goal_time_sec);
+
+  // For moveC via and target can be a Pose or Q. When brace-init style lists are given, values are
+  // interpreted as Pose.
+  instruction_executor->moveC(urcl::Pose{ -0.1, 0.463, 0.559, 0.68, -1.083, -2.076 },
+                              urcl::Pose{ -0.0203, 0.303, 0.559, 0.68, -1.083, -2.076 }, 0.1, 0.1, goal_time_sec);
+  instruction_executor->moveC(urcl::Pose{ -0.1, 0.463, 0.559, 0.68, -1.083, -2.076 },
+                              urcl::Q{ -1.57, -1.83, 1.707, -0.833, 0.782, 0.479 }, 0.1, 0.1, goal_time_sec);
+
+  // For optimove functions, the same target rules as for moveJ and moveL apply.
+  instruction_executor->optimoveJ({ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 }, 1.0, 1.0);
+  instruction_executor->optimoveL(urcl::Pose{ -0.0203, 0.363, 0.559, 0.68, -1.083, -2.076 }, 1.0, 1.0);
+  instruction_executor->optimoveL(urcl::Q{ -1.572, -1.686, 1.707, -0.833, 0.782, 0.479 }, 1.0, 1.0);
+  instruction_executor->optimoveJ(urcl::Pose{ -0.0203, 0.363, 0.559, 0.68, -1.083, -2.076 }, 1.0, 1.0);
 
-  g_my_robot->getUrDriver()->stopControl();
   return 0;
 }
diff --git a/include/ur_client_library/control/motion_primitives.h b/include/ur_client_library/control/motion_primitives.h
@@ -34,6 +34,7 @@
 #include <chrono>
 #include <optional>
 #include <variant>
+#include "ur_client_library/exceptions.h"
 #include <ur_client_library/types.h>
 
 namespace urcl
@@ -51,6 +52,12 @@ enum class MotionType : uint8_t
   OPTIMOVEL = 5,
   MOVEJ_POSE = 6,
   MOVEL_JOINT = 7,
+  MOVEP_JOINT = 8,
+  MOVEC_JOINT = 9,
+  MOVEC_JOINT_POSE = 10,
+  MOVEC_POSE_JOINT = 11,
+  OPTIMOVEJ_POSE = 12,
+  OPTIMOVEL_JOINT = 13,
   SPLINE = 51,
   UNKNOWN = 255
 };
@@ -172,8 +179,31 @@ struct MovePPrimitive : public MotionPrimitive
     this->velocity = velocity;
     this->blend_radius = blend_radius;
   }
+  MovePPrimitive(const MotionTarget& target, const double blend_radius = 0, const double acceleration = 1.4,
+                 const double velocity = 1.04)
+  {
+    std::visit(
+        [&](const auto& target_variant) {
+          using T = std::decay_t<decltype(target_variant)>;
+          if constexpr (std::is_same_v<T, urcl::Pose>)
+          {
+            type = MotionType::MOVEP;
+            target_pose = target_variant;
+          }
+          else if constexpr (std::is_same_v<T, Q>)
+          {
+            type = MotionType::MOVEP_JOINT;
+            target_joint_configuration = target_variant.values;
+          }
+        },
+        target);
+    this->acceleration = acceleration;
+    this->velocity = velocity;
+    this->blend_radius = blend_radius;
+  }
 
   urcl::Pose target_pose;
+  urcl::vector6d_t target_joint_configuration;
 };
 
 struct MoveCPrimitive : public MotionPrimitive
@@ -190,8 +220,65 @@ struct MoveCPrimitive : public MotionPrimitive
     this->mode = mode;
   }
 
+  MoveCPrimitive(const MotionTarget& via_point, const MotionTarget& target, const double blend_radius = 0,
+                 const double acceleration = 1.4, const double velocity = 1.04, const int32_t mode = 0)
+  {
+    if (std::holds_alternative<Q>(via_point))
+    {
+      if (std::holds_alternative<Q>(target))
+      {
+        type = MotionType::MOVEC_JOINT;
+        via_point_pose = urcl::Pose();
+        target_pose = urcl::Pose();
+        via_point_joint_configuration = std::get<Q>(via_point).values;
+        target_joint_configuration = std::get<Q>(target).values;
+      }
+      else if (std::holds_alternative<urcl::Pose>(target))
+      {
+        type = MotionType::MOVEC_POSE_JOINT;
+        via_point_pose = urcl::Pose();
+        target_pose = std::get<urcl::Pose>(target);
+        via_point_joint_configuration = std::get<Q>(via_point).values;
+      }
+      else
+      {
+        throw urcl::UrException("Unhandled motion target type for target point passed to MoveCPrimitive constructor");
+      }
+    }
+    else if (std::holds_alternative<urcl::Pose>(via_point))
+    {
+      if (std::holds_alternative<Q>(target))
+      {
+        type = MotionType::MOVEC_JOINT_POSE;
+        via_point_pose = std::get<urcl::Pose>(via_point);
+        target_pose = urcl::Pose();
+        target_joint_configuration = std::get<Q>(target).values;
+      }
+      else if (std::holds_alternative<urcl::Pose>(target))
+      {
+        type = MotionType::MOVEC;
+        via_point_pose = std::get<urcl::Pose>(via_point);
+        target_pose = std::get<urcl::Pose>(target);
+      }
+      else
+      {
+        throw urcl::UrException("Unhandled motion target type for target point passed to MoveCPrimitive constructor");
+      }
+    }
+    else
+    {
+      throw urcl::UrException("Unhandled motion target type for via_point passed to MoveCPrimitive constructor");
+    }
+    this->acceleration = acceleration;
+    this->velocity = velocity;
+    this->blend_radius = blend_radius;
+    this->mode = mode;
+  }
+
   urcl::Pose via_point_pose;
   urcl::Pose target_pose;
+  urcl::vector6d_t via_point_joint_configuration;
+  urcl::vector6d_t target_joint_configuration;
   int32_t mode = 0;
 };
 
@@ -239,9 +326,33 @@ struct OptimoveJPrimitive : public MotionPrimitive
     this->velocity = velocity_fraction;
   }
 
+  OptimoveJPrimitive(const MotionTarget& target, const double blend_radius = 0,
+                     const double acceleration_fraction = 0.3, const double velocity_fraction = 0.3)
+  {
+    std::visit(
+        [&](const auto& target_variant) {
+          using T = std::decay_t<decltype(target_variant)>;
+          if constexpr (std::is_same_v<T, urcl::Pose>)
+          {
+            type = MotionType::OPTIMOVEJ_POSE;
+            target_pose = target_variant;
+          }
+          else if constexpr (std::is_same_v<T, Q>)
+          {
+            type = MotionType::OPTIMOVEJ;
+            target_joint_configuration = target_variant.values;
+          }
+        },
+        target);
+    this->blend_radius = blend_radius;
+    this->acceleration = acceleration_fraction;
+    this->velocity = velocity_fraction;
+  }
+
   bool validate() const override;
 
   urcl::vector6d_t target_joint_configuration;
+  Pose target_pose;
 };
 
 struct OptimoveLPrimitive : public MotionPrimitive
@@ -255,10 +366,33 @@ struct OptimoveLPrimitive : public MotionPrimitive
     this->acceleration = acceleration_fraction;
     this->velocity = velocity_fraction;
   }
+  OptimoveLPrimitive(const MotionTarget& target, const double blend_radius = 0,
+                     const double acceleration_fraction = 0.3, const double velocity_fraction = 0.3)
+  {
+    std::visit(
+        [&](const auto& target_variant) {
+          using T = std::decay_t<decltype(target_variant)>;
+          if constexpr (std::is_same_v<T, urcl::Pose>)
+          {
+            type = MotionType::OPTIMOVEL;
+            target_pose = target_variant;
+          }
+          else if constexpr (std::is_same_v<T, Q>)
+          {
+            type = MotionType::OPTIMOVEL_JOINT;
+            target_joint_configuration = target_variant.values;
+          }
+        },
+        target);
+    this->blend_radius = blend_radius;
+    this->acceleration = acceleration_fraction;
+    this->velocity = velocity_fraction;
+  }
 
   bool validate() const override;
 
   urcl::Pose target_pose;
+  vector6d_t target_joint_configuration;
 };
 }  // namespace control
 }  // namespace urcl
diff --git a/include/ur_client_library/ur/instruction_executor.h b/include/ur_client_library/ur/instruction_executor.h
@@ -118,6 +118,8 @@ class InstructionExecutor
    */
   bool moveP(const urcl::Pose& target, const double acceleration = 1.4, const double velocity = 1.04,
              const double blend_radius = 0.0);
+  bool moveP(const MotionTarget& target, const double acceleration = 1.4, const double velocity = 1.04,
+             const double blend_radius = 0.0);
 
   /**
    * \brief Move the robot to a pose target using movec
@@ -136,6 +138,8 @@ class InstructionExecutor
    */
   bool moveC(const urcl::Pose& via, const urcl::Pose& target, const double acceleration = 1.4,
              const double velocity = 1.04, const double blend_radius = 0.0, const int32_t mode = 0);
+  bool moveC(const MotionTarget& via, const MotionTarget& target, const double acceleration = 1.4,
+             const double velocity = 1.04, const double blend_radius = 0.0, const int32_t mode = 0);
 
   /**
    * \brief Move the robot to a joint target using optimoveJ.
@@ -154,6 +158,8 @@ class InstructionExecutor
    */
   bool optimoveJ(const urcl::vector6d_t& target, const double acceleration_fraction = 0.3,
                  const double velocity_fraction = 0.3, const double blend_radius = 0);
+  bool optimoveJ(const MotionTarget& target, const double acceleration_fraction = 0.3,
+                 const double velocity_fraction = 0.3, const double blend_radius = 0);
 
   /**
    * \brief Move the robot to a pose target using optimoveL.
@@ -172,6 +178,8 @@ class InstructionExecutor
    */
   bool optimoveL(const urcl::Pose& target, const double acceleration_fraction = 0.3,
                  const double velocity_fraction = 0.3, const double blend_radius = 0);
+  bool optimoveL(const MotionTarget& target, const double acceleration_fraction = 0.3,
+                 const double velocity_fraction = 0.3, const double blend_radius = 0);
 
   /**
    * \brief Cancel the current motion.
diff --git a/resources/external_control.urscript b/resources/external_control.urscript
@@ -42,6 +42,12 @@ MOTION_TYPE_OPTIMOVEJ = 4
 MOTION_TYPE_OPTIMOVEL = 5
 MOTION_TYPE_MOVEJ_POSE = 6
 MOTION_TYPE_MOVEL_JOINT = 7
+MOTION_TYPE_MOVEP_JOINT = 8
+MOTION_TYPE_MOVEC_JOINT = 9
+MOTION_TYPE_MOVEC_JOINT_POSE = 10
+MOTION_TYPE_MOVEC_POSE_JOINT = 11
+MOTION_TYPE_OPTIMOVEJ_POSE = 12
+MOTION_TYPE_OPTIMOVEL_JOINT = 13
 MOTION_TYPE_SPLINE = 51
 TRAJECTORY_DATA_DIMENSION = 3 * 6 + 1
 
@@ -604,11 +610,36 @@ thread trajectoryThread():
         # reset old acceleration
         spline_qdd = [0, 0, 0, 0, 0, 0]
         spline_qd = [0, 0, 0, 0, 0, 0]
+      elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEP_JOINT:
+        acceleration = raw_point[13] / MULT_jointstate
+        velocity = raw_point[7] / MULT_jointstate
+        movep(q, a = acceleration, v = velocity, r = blend_radius)
 
-      elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEC:
+        # reset old acceleration
+        spline_qdd = [0, 0, 0, 0, 0, 0]
+        spline_qd = [0, 0, 0, 0, 0, 0]
+
+      elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEC or
+        raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEC_JOINT or
+        raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEC_POSE_JOINT or
+        raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEC_JOINT_POSE:
         local v = [raw_point[7], raw_point[8], raw_point[9], raw_point[10], raw_point[11], raw_point[12]] / MULT_jointstate
-        via = p[v[0], v[1], v[2], v[3], v[4], v[5]]
-        target = p[q[0], q[1], q[2], q[3], q[4], q[5]]
+        if raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEC:
+          via = p[v[0], v[1], v[2], v[3], v[4], v[5]]
+          target = p[q[0], q[1], q[2], q[3], q[4], q[5]]
+        elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEC_JOINT:
+          via = v
+          target = q
+        elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEC_POSE_JOINT:
+          via = v
+          target = p[q[0], q[1], q[2], q[3], q[4], q[5]]
+        elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEC_JOINT_POSE:
+          via = p[v[0], v[1], v[2], v[3], v[4], v[5]]
+          target = q
+        end
+
+        textmsg(str_cat(str_cat("MOVEC via: ", via), str_cat(" target: ", target)))
+
         acceleration = raw_point[13] / MULT_jointstate
         velocity = raw_point[14] / MULT_jointstate
         mode = raw_point[15] / MULT_jointstate
@@ -643,14 +674,22 @@ thread trajectoryThread():
         end
 
       # OptimoveJ point
-      elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEJ:
+      elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEJ or raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEJ_POSE:
         acceleration = raw_point[13] / MULT_jointstate
         velocity = raw_point[7] / MULT_jointstate
         {% if ROBOT_SOFTWARE_VERSION >= v5.21.0 %}
           {% if ROBOT_SOFTWARE_VERSION < v6.0.0 %}
-        optimovej(q, a = acceleration, v = velocity, r = blend_radius)
+        if raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEJ:
+          optimovej(q, a = acceleration, v = velocity, r = blend_radius)
+        else:
+          optimovej(p[q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, r = blend_radius)
+        end
           {% elif ROBOT_SOFTWARE_VERSION >= v10.8.0 %}
-        optimovej(q, a = acceleration, v = velocity, r = blend_radius)
+        if raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEJ:
+          optimovej(q, a = acceleration, v = velocity, r = blend_radius)
+        else:
+          optimovej(p[q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, r = blend_radius)
+        end
           {% else %}
         popup("Optimovej is only supported from software 10.8.0 and upwards.", error=True, blocking=False)
           {% endif %}
@@ -671,15 +710,23 @@ thread trajectoryThread():
         spline_qd = [0, 0, 0, 0, 0, 0]
 
       # OptimoveL point
-      elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEL:
+      elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEL or raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEL_JOINT:
         acceleration = raw_point[13] / MULT_jointstate
         velocity = raw_point[7] / MULT_jointstate
         
         {% if ROBOT_SOFTWARE_VERSION >= v5.21.0 %}
           {% if ROBOT_SOFTWARE_VERSION < v6.0.0 %}
-        optimovel(p[q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, r = blend_radius)
+        if raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEL:
+          optimovel(p[q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, r = blend_radius)
+        else:
+          optimovel(q, a = acceleration, v = velocity, r = blend_radius)
+        end
           {% elif ROBOT_SOFTWARE_VERSION >= v10.8.0 %}
-        optimovel(p[q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, r = blend_radius)
+        if raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEL:
+          optimovel(p[q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, r = blend_radius)
+        else:
+          optimovel(q, a = acceleration, v = velocity, r = blend_radius)
+        end
           {% else %}
         popup("Optimovel is only supported from software 10.8.0 and upwards.", error=True, blocking=False)
           {% endif %}
diff --git a/src/control/trajectory_point_interface.cpp b/src/control/trajectory_point_interface.cpp
diff --git a/src/ur/instruction_executor.cpp b/src/ur/instruction_executor.cpp
