Support optimove motions in InstructionExecutor

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 speed 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;
+
+  virtual bool validate() const;
 };
 
 struct MoveJPrimitive : public MotionPrimitive
@@ -82,6 +87,7 @@ struct MoveJPrimitive : public MotionPrimitive
     this->blend_radius = blend_radius;
   }
 
+  bool validate() const override;
   urcl::vector6d_t target_joint_configuration;
 };
 
@@ -99,6 +105,8 @@ struct MoveLPrimitive : public MotionPrimitive
     this->blend_radius = blend_radius;
   }
 
+  bool validate() const override;
+
   urcl::Pose target_pose;
 };
 
@@ -113,6 +121,9 @@ struct MovePPrimitive : public MotionPrimitive
     this->velocity = velocity;
     this->blend_radius = blend_radius;
   }
+
+  bool validate() const override;
+
   urcl::Pose target_pose;
 };
 
@@ -129,6 +140,9 @@ struct MoveCPrimitive : public MotionPrimitive
     this->blend_radius = blend_radius;
     this->mode = mode;
   }
+
+  bool validate() const override;
+
   urcl::Pose via_point_pose;
   urcl::Pose target_pose;
   int32_t mode = 0;
@@ -162,6 +176,40 @@ struct SplinePrimitive : public MotionPrimitive
   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;
+  }
+
+  urcl::vector6d_t target_joint_configuration;
+
+  bool validate() const override;
+};
+
+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=True)
+          {% endif %}
+        {% else %}
+        popup("Optimovej is only supported from software 5.21.0 and upwards.", error=True, blocking=True)
+        {% 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=True)
+          {% endif %}
+        {% else %}
+        popup("Optimovel is only supported from software 5.21.0 and upwards.", error=True, blocking=True)
+        {% 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!")