Fix motion primitive validation

Have individual validation functions for each primitive type.

Author: urfeex

include/ur_client_library/control/motion_primitives.h

@@ -63,6 +63,7 @@ enum class TrajectorySplineType : int32_t
 
 struct MotionPrimitive
 {
+  virtual ~MotionPrimitive() = default;
   MotionType type = MotionType::UNKNOWN;
   std::chrono::duration<double> duration;
   double acceleration;
@@ -86,6 +87,7 @@ struct MoveJPrimitive : public MotionPrimitive
     this->blend_radius = blend_radius;
   }
 
+  bool validate() const override;
   urcl::vector6d_t target_joint_configuration;
 };
 
@@ -103,6 +105,8 @@ struct MoveLPrimitive : public MotionPrimitive
     this->blend_radius = blend_radius;
   }
 
+  bool validate() const override;
+
   urcl::Pose target_pose;
 };
 
@@ -117,6 +121,9 @@ struct MovePPrimitive : public MotionPrimitive
     this->velocity = velocity;
     this->blend_radius = blend_radius;
   }
+
+  bool validate() const override;
+
   urcl::Pose target_pose;
 };
 
@@ -133,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;
@@ -202,4 +212,4 @@ struct OptimoveLPrimitive : public MotionPrimitive
 };
 }  // namespace control
 }  // namespace urcl
-#endif  // UR_CLIENT_LIBRARY_MOTION_PRIMITIVES_H_INCLUDED
+#endif  // UR_CLIENT_LIBRARY_MOTION_PRIMITIVES_H_INCLUDED

src/control/motion_primitives.cpp

@@ -36,19 +36,83 @@ 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;
+  }
+  return true;
+}
+bool MoveJPrimitive::validate() const
+{
+  if (!MotionPrimitive::validate())
+  {
+    return false;
+  }
   if (acceleration < 0)
   {
-    URCL_LOG_ERROR("Negative acceleration passed to motion primitive. This is not allowed.");
+    URCL_LOG_ERROR("Negative acceleration passed to MoveJ primitive. This is not allowed.");
     return false;
   }
   if (velocity < 0)
   {
-    URCL_LOG_ERROR("Negative velocity passed to motion primitive. This is not allowed.");
+    URCL_LOG_ERROR("Negative velocity passed to MoveJ primitive. This is not allowed.");
     return false;
   }
-  if (blend_radius < 0)
+  return true;
+}
+
+bool MoveLPrimitive::validate() const
+{
+  if (!MotionPrimitive::validate())
   {
-    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 MoveL primitive. This is not allowed.");
+    return false;
+  }
+  if (velocity < 0)
+  {
+    URCL_LOG_ERROR("Negative velocity passed to MoveL primitive. This is not allowed.");
+    return false;
+  }
+  return true;
+}
+bool MovePPrimitive::validate() const
+{
+  if (!MotionPrimitive::validate())
+  {
+    return false;
+  }
+  if (acceleration < 0)
+  {
+    URCL_LOG_ERROR("Negative acceleration passed to MoveP primitive. This is not allowed.");
+    return false;
+  }
+  if (velocity < 0)
+  {
+    URCL_LOG_ERROR("Negative velocity passed to MoveP primitive. This is not allowed.");
+    return false;
+  }
+  return true;
+}
+
+bool MoveCPrimitive::validate() const
+{
+  if (!MotionPrimitive::validate())
+  {
+    return false;
+  }
+  if (acceleration < 0)
+  {
+    URCL_LOG_ERROR("Negative acceleration passed to MoveC primitive. This is not allowed.");
+    return false;
+  }
+  if (velocity < 0)
+  {
+    URCL_LOG_ERROR("Negative velocity passed to MoveC primitive. This is not allowed.");
     return false;
   }
   return true;