Support optimove motions in InstructionExecutor (#354)

Add support for optimovej and optimovel in the InstructionExecutor.

Author: urfeex

.github/workflows/ci.yml

@@ -10,7 +10,7 @@ on:
 
 jobs:
   build:
-    timeout-minutes: 30
+    timeout-minutes: 60
     runs-on: ubuntu-latest
     name: build (${{matrix.env.URSIM_VERSION}}-${{matrix.env.ROBOT_MODEL}})
     strategy:

CMakeLists.txt

@@ -17,6 +17,7 @@ endif()
 add_library(urcl
     src/comm/tcp_socket.cpp
     src/comm/tcp_server.cpp
+    src/control/motion_primitives.cpp
     src/control/reverse_interface.cpp
     src/control/script_reader.cpp
     src/control/script_sender.cpp

doc/architecture/instruction_executor.rst

@@ -12,6 +12,8 @@ to point motions easily accessible. Currently, it supports the following instruc
 * Execute MoveL point to point motions
 * Execute MoveP point to point motions
 * Execute MoveC circular motions
+* Execute OptimoveJ point to point motions (For PolyScope 5.21 / PolyScope 10.8 and later)
+* Execute OptimoveL point to point motions (For PolyScope 5.21 / PolyScope 10.8 and later)
 * Execute sequences consisting of the motion primitives above
 
 The Instruction Executor uses the :ref:`trajectory_point_interface` and the

doc/examples/instruction_executor.rst

@@ -44,10 +44,11 @@ To run a sequence of motions, create an
    :end-at: instruction_executor->executeMotion(motion_sequence);
 
 Each element in the motion sequence can be a different motion type. In the example, there are two
-``MoveJ`` motions, a ``MoveL`` motion and a ``MoveP`` motion. The primitives' parameters are directly forwarded to
-the underlying script functions, so the parameter descriptions for them apply, as well.
-Particularly, you may want to choose between either a time-based execution speed or an acceleration
-/ velocity parametrization for some move functions. The latter will be ignored if a time > 0 is given.
+``MoveJ`` motions, a ``MoveL`` motion, a ``MoveP`` motion, a ``OptimiveJ`` motion and a
+``OptimoveL`` motion. The primitives' parameters are directly forwarded to the underlying script
+functions, so the parameter descriptions for them apply, as well. Particularly, you may want to
+choose between either a time-based execution or an acceleration / velocity parametrization
+for some move functions. The latter will be ignored if a time > 0 is given.
 
 Please refer to the script manual for details.
 

examples/instruction_executor.cpp

@@ -91,6 +91,10 @@ int main(int argc, char* argv[])
                                                     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,
+                                                        0.7),
+    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);
 
@@ -109,4 +113,4 @@ int main(int argc, char* argv[])
 
   g_my_robot->getUrDriver()->stopControl();
   return 0;
-}
+}

include/ur_client_library/control/motion_primitives.h

@@ -46,6 +46,8 @@ enum class MotionType : uint8_t
   MOVEL = 1,
   MOVEP = 2,
   MOVEC = 3,
+  OPTIMOVEJ = 4,
+  OPTIMOVEL = 5,
   SPLINE = 51,
   UNKNOWN = 255
 };
@@ -61,11 +63,14 @@ enum class TrajectorySplineType : int32_t
 
 struct MotionPrimitive
 {
+  virtual ~MotionPrimitive() = default;
   MotionType type = MotionType::UNKNOWN;
   std::chrono::duration<double> duration;
   double acceleration;
   double velocity;
-  double blend_radius;
+  double blend_radius = 0.0;
+
+  virtual bool validate() const;
 };
 
 struct MoveJPrimitive : public MotionPrimitive
@@ -113,6 +118,7 @@ struct MovePPrimitive : public MotionPrimitive
     this->velocity = velocity;
     this->blend_radius = blend_radius;
   }
+
   urcl::Pose target_pose;
 };
 
@@ -129,6 +135,7 @@ struct MoveCPrimitive : public MotionPrimitive
     this->blend_radius = blend_radius;
     this->mode = mode;
   }
+
   urcl::Pose via_point_pose;
   urcl::Pose target_pose;
   int32_t mode = 0;
@@ -158,10 +165,47 @@ struct SplinePrimitive : public MotionPrimitive
       return control::TrajectorySplineType::SPLINE_CUBIC;
     }
   }
+
+  bool validate() const override;
+
   vector6d_t target_positions;
   vector6d_t target_velocities;
   std::optional<vector6d_t> target_accelerations;
 };
+
+struct OptimoveJPrimitive : public MotionPrimitive
+{
+  OptimoveJPrimitive(const urcl::vector6d_t& target, const double blend_radius = 0,
+                     const double acceleration_fraction = 0.3, const double velocity_fraction = 0.3)
+  {
+    type = MotionType::OPTIMOVEJ;
+    target_joint_configuration = target;
+    this->blend_radius = blend_radius;
+    this->acceleration = acceleration_fraction;
+    this->velocity = velocity_fraction;
+  }
+
+  bool validate() const override;
+
+  urcl::vector6d_t target_joint_configuration;
+};
+
+struct OptimoveLPrimitive : public MotionPrimitive
+{
+  OptimoveLPrimitive(const urcl::Pose& target, const double blend_radius = 0, const double acceleration_fraction = 0.3,
+                     const double velocity_fraction = 0.3)
+  {
+    type = MotionType::OPTIMOVEL;
+    target_pose = target;
+    this->blend_radius = blend_radius;
+    this->acceleration = acceleration_fraction;
+    this->velocity = velocity_fraction;
+  }
+
+  bool validate() const override;
+
+  urcl::Pose target_pose;
+};
 }  // namespace control
 }  // namespace urcl
-#endif  // UR_CLIENT_LIBRARY_MOTION_PRIMITIVES_H_INCLUDED
+#endif  // UR_CLIENT_LIBRARY_MOTION_PRIMITIVES_H_INCLUDED

include/ur_client_library/ur/instruction_executor.h

@@ -132,6 +132,42 @@ class InstructionExecutor
   bool moveC(const urcl::Pose& via, const urcl::Pose& target, const double acceleration = 1.4,
              const double velocity = 1.04, const double blend_radius = 0.0, const int32_t mode = 0);
 
+  /**
+   * \brief Move the robot to a joint target using optimoveJ.
+   *
+   * This function will move the robot to the given joint target using the optimoveJ motion
+   * primitive. The robot will move with the given acceleration and velocity fractions.
+   *
+   * \param target The joint target to move to.
+   * \param acceleration_fraction The fraction of the maximum acceleration to use for the motion
+   * (0.0 < fraction <= 1.0).
+   * \param velocity_fraction The fraction of the maximum velocity to use for the motion_sequence
+   * (0.0 < fraction <= 1.0).
+   * \param blend_radius The blend radius to use for the motion.
+   *
+   * \return True if the robot has reached the target, false otherwise.
+   */
+  bool optimoveJ(const urcl::vector6d_t& target, const double acceleration_fraction = 0.3,
+                 const double velocity_fraction = 0.3, const double blend_radius = 0);
+
+  /**
+   * \brief Move the robot to a pose target using optimoveL.
+   *
+   * This function will move the robot to the given pose target using the optimoveL motion
+   * primitive. The robot will move with the given acceleration and velocity fractions.
+   *
+   * \param target The pose target to move to.
+   * \param acceleration_fraction The fraction of the maximum acceleration to use for the motion
+   * (0.0 < fraction <= 1.0).
+   * \param velocity_fraction The fraction of the maximum velocity to use for the motion_sequence
+   * (0.0 < fraction <= 1.0).
+   * \param blend_radius The blend radius to use for the motion.
+   *
+   * \return True if the robot has reached the target, false otherwise.
+   */
+  bool optimoveL(const urcl::Pose& target, const double acceleration_fraction = 0.3,
+                 const double velocity_fraction = 0.3, const double blend_radius = 0);
+
   /**
    * \brief Cancel the current motion.
    *
@@ -154,6 +190,7 @@ class InstructionExecutor
 private:
   void trajDoneCallback(const urcl::control::TrajectoryResult& result);
   void trajDisconnectCallback(const int filedescriptor);
+
   std::shared_ptr<urcl::UrDriver> driver_;
   std::atomic<bool> trajectory_running_ = false;
   std::atomic<bool> cancel_requested_ = false;

resources/external_control.urscript

@@ -8,6 +8,8 @@ textmsg("ExternalControl: steptime=", steptime)
 MULT_jointstate = {{JOINT_STATE_REPLACE}}
 MULT_time = {{TIME_REPLACE}}
 
+DEBUG = False
+
 STOPJ_ACCELERATION = 4.0
 
 #Constants
@@ -35,6 +37,8 @@ MOTION_TYPE_MOVEJ = 0
 MOTION_TYPE_MOVEL = 1
 MOTION_TYPE_MOVEP = 2
 MOTION_TYPE_MOVEC = 3
+MOTION_TYPE_OPTIMOVEJ = 4
+MOTION_TYPE_OPTIMOVEL = 5
 MOTION_TYPE_SPLINE = 51
 TRAJECTORY_DATA_DIMENSION = 3 * 6 + 1
 
@@ -551,6 +555,63 @@ thread trajectoryThread():
           clear_remaining_trajectory_points()
           trajectory_result = TRAJECTORY_RESULT_FAILURE
         end
+
+      # OptimoveJ point
+      elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEJ:
+        acceleration = raw_point[13] / MULT_jointstate
+        velocity = raw_point[7] / MULT_jointstate
+        {% if ROBOT_SOFTWARE_VERSION >= v5.21.0 %}
+          {% if ROBOT_SOFTWARE_VERSION < v6.0.0 %}
+        optimovej(q, a = acceleration, v = velocity, r = blend_radius)
+          {% elif ROBOT_SOFTWARE_VERSION >= v10.8.0 %}
+        optimovej(q, a = acceleration, v = velocity, r = blend_radius)
+          {% else %}
+        popup("Optimovej is only supported from software 10.8.0 and upwards.", error=True, blocking=False)
+          {% endif %}
+        {% else %}
+        popup("Optimovej is only supported from software 5.21.0 and upwards.", error=True, blocking=False)
+        {% endif %}
+
+        if DEBUG:
+          textmsg(str_cat("optimovej(", str_cat(
+            str_cat("q=", q), str_cat(
+              str_cat(", a=", acceleration), str_cat(
+                str_cat(", v=", velocity), str_cat(
+                  str_cat(", r=", blend_radius), ")"))))))
+        end
+
+        # reset old acceleration
+        spline_qdd = [0, 0, 0, 0, 0, 0]
+        spline_qd = [0, 0, 0, 0, 0, 0]
+
+      # OptimoveL point
+      elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEL:
+        acceleration = raw_point[13] / MULT_jointstate
+        velocity = raw_point[7] / MULT_jointstate
+        
+        {% if ROBOT_SOFTWARE_VERSION >= v5.21.0 %}
+          {% if ROBOT_SOFTWARE_VERSION < v6.0.0 %}
+        optimovel(p[q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, r = blend_radius)
+          {% elif ROBOT_SOFTWARE_VERSION >= v10.8.0 %}
+        optimovel(p[q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, r = blend_radius)
+          {% else %}
+        popup("Optimovel is only supported from software 10.8.0 and upwards.", error=True, blocking=False)
+          {% endif %}
+        {% else %}
+        popup("Optimovel is only supported from software 5.21.0 and upwards.", error=True, blocking=False)
+        {% endif %}
+
+        if DEBUG:
+          textmsg(str_cat("optimovel(", str_cat(
+            str_cat("q=", p[q[0], q[1], q[2], q[3], q[4], q[5]]), str_cat(
+              str_cat(", a=", acceleration), str_cat(
+                str_cat(", v=", velocity), str_cat(
+                  str_cat(", r=", blend_radius), ")"))))))
+        end
+
+        # reset old acceleration
+        spline_qdd = [0, 0, 0, 0, 0, 0]
+        spline_qd = [0, 0, 0, 0, 0, 0]
       end
     else:
       textmsg("Receiving trajectory point failed!")
@@ -815,4 +876,4 @@ socket_close("reverse_socket")
 socket_close("trajectory_socket")
 socket_close("script_command_socket")
 
-# NODE_CONTROL_LOOP_ENDS
+# NODE_CONTROL_LOOP_ENDS

src/control/motion_primitives.cpp

@@ -0,0 +1,100 @@
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// Copyright 2025 Universal Robots A/S
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the {copyright_holder} nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+// -- END LICENSE BLOCK ------------------------------------------------
+
+#include <ur_client_library/control/motion_primitives.h>
+#include <ur_client_library/log.h>
+
+namespace urcl::control
+{
+
+bool MotionPrimitive::validate() const
+{
+  if (blend_radius < 0)
+  {
+    URCL_LOG_ERROR("Negative blend radius passed to motion primitive. This is not allowed.");
+    return false;
+  }
+  if (acceleration < 0)
+  {
+    URCL_LOG_ERROR("Negative acceleration passed to motion primitive. This is not allowed.");
+    return false;
+  }
+  if (velocity < 0)
+  {
+    URCL_LOG_ERROR("Negative velocity passed to motion primitive. This is not allowed.");
+    return false;
+  }
+  return true;
+}
+
+bool SplinePrimitive::validate() const
+{
+  // Spline primitives don't have the same restriction as others do. Whether the primitives are valid or not
+  // is checked in the URScript program.
+  return true;
+}
+bool OptimoveJPrimitive::validate() const
+{
+  if (!MotionPrimitive::validate())
+  {
+    return false;
+  }
+  if (acceleration <= 0 || acceleration > 1.0)
+  {
+    URCL_LOG_ERROR("Acceleration fraction must be in range (0, 1].");
+    return false;
+  }
+  if (velocity <= 0 || velocity > 1.0)
+  {
+    URCL_LOG_ERROR("Velocity fraction must be in range (0, 1].");
+    return false;
+  }
+  return true;
+}
+
+bool OptimoveLPrimitive::validate() const
+{
+  if (!MotionPrimitive::validate())
+  {
+    return false;
+  }
+  if (acceleration <= 0 || acceleration > 1.0)
+  {
+    URCL_LOG_ERROR("Acceleration fraction must be in range (0, 1].");
+    return false;
+  }
+  if (velocity <= 0 || velocity > 1.0)
+  {
+    URCL_LOG_ERROR("Velocity fraction must be in range (0, 1].");
+    return false;
+  }
+  return true;
+}
+}  // namespace urcl::control