Support optimove motions in InstructionExecutor

Author: urfeex

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

@@ -58,7 +58,7 @@ int main(int argc, char* argv[])
     robot_ip = std::string(argv[1]);
   }
 
-  bool headless_mode = true;
+  bool headless_mode = false;
   g_my_robot = std::make_unique<urcl::ExampleRobotWrapper>(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode,
                                                            "external_control.urp");
   if (!g_my_robot->getUrDriver()->checkCalibration(CALIBRATION_CHECKSUM))
@@ -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);
 

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
 };
@@ -66,6 +68,8 @@ struct MotionPrimitive
   double acceleration;
   double velocity;
   double blend_radius;
+
+  virtual bool validate() const;
 };
 
 struct MoveJPrimitive : public MotionPrimitive
@@ -162,6 +166,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

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!")

src/control/trajectory_point_interface.cpp

@@ -62,6 +62,12 @@ TrajectoryPointInterface::TrajectoryPointInterface(uint32_t port) : ReverseInter
 
 bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptr<control::MotionPrimitive> primitive)
 {
+  if (!primitive->validate())
+  {
+    URCL_LOG_ERROR("Motion primitive validation failed.");
+    return false;
+  }
+
   if (client_fd_ == -1)
   {
     return false;
@@ -131,6 +137,25 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptr<contro
       }
       break;
     }
+    case control::MotionType::OPTIMOVEJ:
+    {
+      auto optimovej_primitive = std::static_pointer_cast<control::OptimoveJPrimitive>(primitive);
+      first_block = optimovej_primitive->target_joint_configuration;
+      second_block.fill(primitive->velocity);
+      third_block.fill(primitive->acceleration);
+      break;
+    }
+    case control::MotionType::OPTIMOVEL:
+    {
+      URCL_LOG_INFO("Executing OptimoveL motion.");
+      auto optimovel_primitive = std::static_pointer_cast<control::OptimoveLPrimitive>(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();
   }

src/ur/instruction_executor.cpp

@@ -99,6 +99,7 @@ bool urcl::InstructionExecutor::moveJ(const urcl::vector6d_t& target, const doub
   return executeMotion({ std::make_shared<control::MoveJPrimitive>(
       target, blend_radius, std::chrono::milliseconds(static_cast<int>(time * 1000)), acceleration, velocity) });
 }
+
 bool urcl::InstructionExecutor::moveL(const urcl::Pose& target, const double acceleration, const double velocity,
                                       const double time, const double blend_radius)
 {
@@ -119,6 +120,18 @@ bool urcl::InstructionExecutor::moveC(const urcl::Pose& via, const urcl::Pose& t
       { std::make_shared<control::MoveCPrimitive>(via, target, blend_radius, acceleration, velocity, mode) });
 }
 
+bool urcl::InstructionExecutor::optimoveJ(const urcl::vector6d_t& target, const double acceleration,
+                                          const double velocity, const double blend_radius)
+{
+  return executeMotion({ std::make_shared<control::OptimoveJPrimitive>(target, blend_radius, acceleration, velocity) });
+}
+
+bool urcl::InstructionExecutor::optimoveL(const urcl::Pose& target, const double acceleration, const double velocity,
+                                          const double blend_radius)
+{
+  return executeMotion({ std::make_shared<control::OptimoveLPrimitive>(target, blend_radius, acceleration, velocity) });
+}
+
 bool urcl::InstructionExecutor::cancelMotion()
 {
   cancel_requested_ = true;

src/ur/ur_driver.cpp

@@ -127,6 +127,9 @@ void UrDriver::init(const UrDriverConfiguration& config)
                   << config.tool_comm_setup->getTxIdleChars() << ")";
   }
   data[BEGIN_REPLACE] = begin_replace.str();
+
+  data["ROBOT_SOFTWARE_VERSION"] = getVersion();
+
   script_reader_.reset(new control::ScriptReader());
   std::string prog = script_reader_->readScriptFile(config.script_file, data);