Self-review

Author: urfeex

include/ur_client_library/control/motion_primitives.h

@@ -34,7 +34,6 @@
 #include <chrono>
 #include <optional>
 #include <variant>
-#include "ur_client_library/exceptions.h"
 #include <ur_client_library/types.h>
 
 namespace urcl
@@ -76,6 +75,45 @@ enum class MotionType : uint8_t
   UNKNOWN = 255
 };
 
+inline std::string motionTypeToString(const MotionType type)
+{
+  switch (type)
+  {
+    case MotionType::MOVEJ:
+      return "MOVEJ";
+    case MotionType::MOVEL:
+      return "MOVEL";
+    case MotionType::MOVEP:
+      return "MOVEP";
+    case MotionType::MOVEC:
+      return "MOVEC";
+    case MotionType::OPTIMOVEJ:
+      return "OPTIMOVEJ";
+    case MotionType::OPTIMOVEL:
+      return "OPTIMOVEL";
+    case MotionType::MOVEJ_POSE:
+      return "MOVEJ_POSE";
+    case MotionType::MOVEL_JOINT:
+      return "MOVEL_JOINT";
+    case MotionType::MOVEP_JOINT:
+      return "MOVEP_JOINT";
+    case MotionType::MOVEC_JOINT:
+      return "MOVEC_JOINT";
+    case MotionType::MOVEC_JOINT_POSE:
+      return "MOVEC_JOINT_POSE";
+    case MotionType::MOVEC_POSE_JOINT:
+      return "MOVEC_POSE_JOINT";
+    case MotionType::OPTIMOVEJ_POSE:
+      return "OPTIMOVEJ_POSE";
+    case MotionType::OPTIMOVEL_JOINT:
+      return "OPTIMOVEL_JOINT";
+    case MotionType::SPLINE:
+      return "SPLINE";
+    default:
+      return "UNKNOWN";
+  }
+}
+
 /*!
  * Spline types
  */
@@ -144,8 +182,8 @@ struct MoveJPrimitive : public MotionPrimitive
         target);
   }
 
-  urcl::vector6d_t target_joint_configuration;
-  urcl::Pose target_pose;
+  urcl::vector6d_t target_joint_configuration{};
+  urcl::Pose target_pose{};
 };
 
 struct MoveLPrimitive : public MotionPrimitive
@@ -195,8 +233,8 @@ struct MoveLPrimitive : public MotionPrimitive
         target);
   }
 
-  urcl::Pose target_pose;
-  urcl::vector6d_t target_joint_configuration;
+  urcl::Pose target_pose{};
+  urcl::vector6d_t target_joint_configuration{};
 };
 
 struct MovePPrimitive : public MotionPrimitive
@@ -239,8 +277,8 @@ struct MovePPrimitive : public MotionPrimitive
     this->blend_radius = blend_radius;
   }
 
-  urcl::Pose target_pose;
-  urcl::vector6d_t target_joint_configuration;
+  urcl::Pose target_pose{};
+  urcl::vector6d_t target_joint_configuration{};
 };
 
 struct MoveCPrimitive : public MotionPrimitive
@@ -261,73 +299,62 @@ struct MoveCPrimitive : public MotionPrimitive
    * \brief Construct a MoveC primitive from two \ref urcl::MotionTarget values.
    *
    * Every combination of \ref urcl::Pose and \ref urcl::Q for the via point and the target is
-   * supported and mapped to the corresponding \ref MotionType (``MOVEC``,
-   * ``MOVEC_JOINT``, ``MOVEC_POSE_JOINT``, or ``MOVEC_JOINT_POSE``). When a joint configuration
-   * is used for either role, only the corresponding joint member is populated and the pose
-   * member is set to a default-constructed \ref urcl::Pose.
+   * supported and mapped to the corresponding \ref MotionType (``MOVEC``, ``MOVEC_JOINT``,
+   * ``MOVEC_POSE_JOINT``, or ``MOVEC_JOINT_POSE``). The naming convention is
+   * ``MOVEC_<target>_<via>``, i.e. ``MOVEC_POSE_JOINT`` denotes a movec whose target is a pose
+   * and whose via point is a joint configuration.
    *
-   * \throws urcl::UrException if either variant is ever extended with an alternative that is
-   * not handled here.
+   * Unhandled variant alternatives are caught at compile time via ``static_assert``.
    */
   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");
-    }
+    std::visit(
+        [&](const auto& via_variant, const auto& target_variant) {
+          using ViaT = std::decay_t<decltype(via_variant)>;
+          using TargetT = std::decay_t<decltype(target_variant)>;
+          static_assert(std::is_same_v<ViaT, Q> || std::is_same_v<ViaT, urcl::Pose>, "Unhandled MotionTarget "
+                                                                                     "alternative for via_point");
+          static_assert(std::is_same_v<TargetT, Q> || std::is_same_v<TargetT, urcl::Pose>, "Unhandled MotionTarget "
+                                                                                           "alternative for target");
+
+          if constexpr (std::is_same_v<ViaT, urcl::Pose> && std::is_same_v<TargetT, urcl::Pose>)
+          {
+            type = MotionType::MOVEC;
+            via_point_pose = via_variant;
+            target_pose = target_variant;
+          }
+          else if constexpr (std::is_same_v<ViaT, Q> && std::is_same_v<TargetT, Q>)
+          {
+            type = MotionType::MOVEC_JOINT;
+            via_point_joint_configuration = via_variant.values;
+            target_joint_configuration = target_variant.values;
+          }
+          else if constexpr (std::is_same_v<ViaT, Q> && std::is_same_v<TargetT, urcl::Pose>)
+          {
+            type = MotionType::MOVEC_POSE_JOINT;
+            via_point_joint_configuration = via_variant.values;
+            target_pose = target_variant;
+          }
+          else if constexpr (std::is_same_v<ViaT, urcl::Pose> && std::is_same_v<TargetT, Q>)
+          {
+            type = MotionType::MOVEC_JOINT_POSE;
+            via_point_pose = via_variant;
+            target_joint_configuration = target_variant.values;
+          }
+        },
+        via_point, target);
+
     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;
+  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;
 };
 
@@ -406,8 +433,8 @@ struct OptimoveJPrimitive : public MotionPrimitive
 
   bool validate() const override;
 
-  urcl::vector6d_t target_joint_configuration;
-  Pose target_pose;
+  urcl::vector6d_t target_joint_configuration{};
+  Pose target_pose{};
 };
 
 struct OptimoveLPrimitive : public MotionPrimitive
@@ -452,8 +479,8 @@ struct OptimoveLPrimitive : public MotionPrimitive
 
   bool validate() const override;
 
-  urcl::Pose target_pose;
-  vector6d_t target_joint_configuration;
+  urcl::Pose target_pose{};
+  vector6d_t target_joint_configuration{};
 };
 }  // namespace control
 }  // namespace urcl

include/ur_client_library/types.h

@@ -50,6 +50,9 @@ struct Q
   constexpr Q(double q1, double q2, double q3, double q4, double q5, double q6) : values{ q1, q2, q3, q4, q5, q6 }
   {
   }
+  explicit constexpr Q(const vector6d_t& values) : values(values)
+  {
+  }
 
   vector6d_t values;
 };

resources/external_control.urscript

@@ -578,7 +578,7 @@ thread trajectoryThread():
         acceleration = raw_point[13] / MULT_jointstate
         velocity = raw_point[7] / MULT_jointstate
         movej(p[q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, t = tmptime, r = blend_radius)
- 
+
         # reset old acceleration
         spline_qdd = [0, 0, 0, 0, 0, 0]
         spline_qd = [0, 0, 0, 0, 0, 0]
@@ -595,12 +595,12 @@ thread trajectoryThread():
       elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEL_JOINT:
         acceleration = raw_point[13] / MULT_jointstate
         velocity = raw_point[7] / MULT_jointstate
-        movel([q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, t = tmptime, r = blend_radius)
+        movel(q, a = acceleration, v = velocity, t = tmptime, r = blend_radius)
 
         # reset old acceleration
         spline_qdd = [0, 0, 0, 0, 0, 0]
         spline_qd = [0, 0, 0, 0, 0, 0]
-        
+
       # MoveP point
       elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEP:
         acceleration = raw_point[13] / MULT_jointstate
@@ -711,7 +711,7 @@ thread trajectoryThread():
       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 %}
         if raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEL:
@@ -766,8 +766,8 @@ thread clearTrajectoryPointsThread():
     limit = 20
   {% else %}
     limit = 10
-  {% endif %}  
-  
+  {% endif %}
+
   while trajectory_points_left > 0:
     raw_point = socket_read_binary_integer(TRAJECTORY_DATA_DIMENSION + 2, "trajectory_socket", timeout)
     if raw_point[0] <= 0:
@@ -778,7 +778,7 @@ thread clearTrajectoryPointsThread():
     trajectory_points_left = trajectory_points_left - 1
     reads = reads + 1
     if reads >= limit:
-      sync()            
+      sync()
       reads = 0
     end
   end
@@ -999,17 +999,17 @@ end
 
 # NODE_CONTROL_LOOP_BEGINS
 
-# Attempt to establish the three required socket connections, if at least one socket is still disconnected, 
+# Attempt to establish the three required socket connections, if at least one socket is still disconnected,
 # a popup is shown and the loop continues until all three flags are True.
 traj_flag = False
 com_flag = False
 rev_flag = False
 while not (traj_flag and com_flag and rev_flag):
-  if not traj_flag: 
+  if not traj_flag:
     traj_flag = connect_socket_with_retry("{{SERVER_IP_REPLACE}}", {{TRAJECTORY_SERVER_PORT_REPLACE}}, "trajectory_socket")
   end
 
-  if traj_flag and not com_flag: 
+  if traj_flag and not com_flag:
     com_flag = connect_socket_with_retry("{{SERVER_IP_REPLACE}}", {{SCRIPT_COMMAND_SERVER_PORT_REPLACE}}, "script_command_socket")
   end
 
@@ -1052,7 +1052,7 @@ while control_mode > MODE_STOPPED:
   if params_mult[0] > 0:
 
     # Convert read timeout from milliseconds to seconds
-    read_timeout = params_mult[1] / 1000.0 
+    read_timeout = params_mult[1] / 1000.0
 
     if control_mode != params_mult[REVERSE_INTERFACE_DATA_DIMENSION]:
       # Clear remaining trajectory points

src/control/trajectory_point_interface.cpp

@@ -104,7 +104,8 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptr<contro
       }
       else
       {
-        throw InvalidData("Motion type does not match motion primitive type.");
+        throw InvalidData("Motion type " + motionTypeToString(movej_primitive->type) +
+                          " is not allowed in a MoveJPrimitive.");
       }
       second_block.fill(primitive->velocity);
       third_block.fill(primitive->acceleration);
@@ -126,7 +127,8 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptr<contro
       }
       else
       {
-        throw InvalidData("Motion type does not match motion primitive type.");
+        throw InvalidData("Motion type " + motionTypeToString(movel_primitive->type) +
+                          " is not allowed in a MoveLPrimitive.");
       }
       second_block.fill(primitive->velocity);
       third_block.fill(primitive->acceleration);
@@ -146,6 +148,11 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptr<contro
       {
         first_block = movep_primitive->target_joint_configuration;
       }
+      else
+      {
+        throw InvalidData("Motion type " + motionTypeToString(movep_primitive->type) +
+                          " is not allowed in a MovePPrimitive.");
+      }
       second_block.fill(primitive->velocity);
       third_block.fill(primitive->acceleration);
       break;
@@ -184,6 +191,11 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptr<contro
                          movec_primitive->via_point_pose.z,  movec_primitive->via_point_pose.rx,
                          movec_primitive->via_point_pose.ry, movec_primitive->via_point_pose.rz };
       }
+      else
+      {
+        throw InvalidData("Motion type " + motionTypeToString(movec_primitive->type) +
+                          " is not allowed in a MoveCPrimitive.");
+      }
       third_block = {
         primitive->velocity, primitive->acceleration, static_cast<double>(movec_primitive->mode), 0, 0, 0
       };
@@ -214,6 +226,11 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptr<contro
                         optimovej_primitive->target_pose.z,  optimovej_primitive->target_pose.rx,
                         optimovej_primitive->target_pose.ry, optimovej_primitive->target_pose.rz };
       }
+      else
+      {
+        throw InvalidData("Motion type " + motionTypeToString(optimovej_primitive->type) +
+                          " is not allowed in an OptimoveJPrimitive.");
+      }
       second_block.fill(primitive->velocity);
       third_block.fill(primitive->acceleration);
       break;
@@ -232,6 +249,11 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptr<contro
       {
         first_block = optimovel_primitive->target_joint_configuration;
       }
+      else
+      {
+        throw InvalidData("Motion type " + motionTypeToString(optimovel_primitive->type) +
+                          " is not allowed in an OptimoveLPrimitive.");
+      }
       second_block.fill(primitive->velocity);
       third_block.fill(primitive->acceleration);
       break;