Add MoveP to trajectory_point_interface

urfeex · urfeex · commit 4c5c300b8424 · 2025-04-01T14:59:35.000+02:00
diff --git a/include/ur_client_library/control/motion_primitives.h b/include/ur_client_library/control/motion_primitives.h
@@ -48,9 +48,10 @@ enum class MotionType : uint8_t
   SPLINE = 51,
   UNKNOWN = 255
 };
+
 struct MotionPrimitive
 {
-  MotionType type;
+  MotionType type = MotionType::UNKNOWN;
   std::chrono::duration<double> duration;
   double acceleration;
   double velocity;
@@ -90,6 +91,36 @@ struct MoveLPrimitive : public MotionPrimitive
 
   urcl::Pose target_pose;
 };
+
+struct MovePPrimitive : public MotionPrimitive
+{
+  MovePPrimitive(const urcl::Pose& target, const double blend_radius = 0, const double acceleration = 1.4,
+                 const double velocity = 1.04)
+  {
+    type = MotionType::MOVEP;
+    target_pose = target;
+    this->acceleration = acceleration;
+    this->velocity = velocity;
+    this->blend_radius = blend_radius;
+  }
+  urcl::Pose target_pose;
+};
+
+struct MoveCPrimitive : public MotionPrimitive
+{
+  MoveCPrimitive(const urcl::Pose& via_point, const urcl::Pose& target, const double blend_radius = 0,
+                 const double acceleration = 1.4, const double velocity = 1.04)
+  {
+    type = MotionType::MOVEC;
+    via_point_pose = via_point;
+    target_pose = target;
+    this->acceleration = acceleration;
+    this->velocity = velocity;
+    this->blend_radius = blend_radius;
+  }
+  urcl::Pose via_point_pose;
+  urcl::Pose target_pose;
+};
 }  // namespace control
 }  // namespace urcl
-#endif  // UR_CLIENT_LIBRARY_MOTION_PRIMITIVES_H_INCLUDED
+#endif  // UR_CLIENT_LIBRARY_MOTION_PRIMITIVES_H_INCLUDED
diff --git a/include/ur_client_library/control/trajectory_point_interface.h b/include/ur_client_library/control/trajectory_point_interface.h
@@ -31,6 +31,7 @@
 
 #include <optional>
 
+#include "ur_client_library/control/motion_primitives.h"
 #include "ur_client_library/control/reverse_interface.h"
 #include "ur_client_library/comm/control_mode.h"
 #include "ur_client_library/types.h"
@@ -131,6 +132,8 @@ class TrajectoryPointInterface : public ReverseInterface
   bool writeTrajectorySplinePoint(const vector6d_t* positions, const vector6d_t* velocities,
                                   const vector6d_t* accelerations, const float goal_time);
 
+  bool writeMotionPrimitive(const std::shared_ptr<control::MotionPrimitive> primitive);
+
   void setTrajectoryEndCallback(std::function<void(TrajectoryResult)> callback)
   {
     handle_trajectory_end_ = callback;
diff --git a/include/ur_client_library/exceptions.h b/include/ur_client_library/exceptions.h
@@ -205,5 +205,13 @@ class MissingArgument : public UrException
     return text_.c_str();
   }
 };
+
+class UnsupportedMotionType : public UrException
+{
+public:
+  explicit UnsupportedMotionType() : std::runtime_error("Unsupported motion type")
+  {
+  }
+};
 }  // namespace urcl
 #endif  // ifndef UR_CLIENT_LIBRARY_EXCEPTIONS_H_INCLUDED
diff --git a/include/ur_client_library/types.h b/include/ur_client_library/types.h
@@ -46,6 +46,11 @@ struct Pose
   double rx;
   double ry;
   double rz;
+
+  bool operator==(const Pose& other) const
+  {
+    return x == other.x && y == other.y && z == other.z && rx == other.rx && ry == other.ry && rz == other.rz;
+  }
 };
 
 template <class T, std::size_t N>
diff --git a/include/ur_client_library/ur/instruction_executor.h b/include/ur_client_library/ur/instruction_executor.h
@@ -98,6 +98,12 @@ class InstructionExecutor
   bool moveL(const urcl::Pose& target, const double acceleration = 1.4, const double velocity = 1.04,
              const double time = 0, const double blend_radius = 0);
 
+  bool moveP(const urcl::Pose& target, const double acceleration = 1.4, const double velocity = 1.04,
+             const double blend_radius = 0);
+
+  bool moveC(const urcl::Pose& via_point, const urcl::Pose& target, const double acceleration = 1.4,
+             const double velocity = 1.04, const double blend_radius = 0);
+
   /**
    * \brief Cancel the current motion.
    *
diff --git a/resources/external_control.urscript b/resources/external_control.urscript
@@ -33,6 +33,8 @@ TRAJECTORY_MODE_CANCEL = -1
 
 MOTION_TYPE_MOVEJ = 0
 MOTION_TYPE_MOVEL = 1
+MOTION_TYPE_MOVEP = 2
+MOTION_TYPE_MOVEC = 3
 MOTION_TYPE_SPLINE = 51
 TRAJECTORY_DATA_DIMENSION = 3 * 6 + 1
 
@@ -504,6 +506,16 @@ thread trajectoryThread():
         # 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
+        velocity = raw_point[7] / MULT_jointstate
+        movep(p[q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, r = blend_radius)
+
+        # reset old acceleration
+        spline_qdd = [0, 0, 0, 0, 0, 0]
+        spline_qd = [0, 0, 0, 0, 0, 0]
 
         # Joint spline point
       elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_SPLINE:
diff --git a/src/control/trajectory_point_interface.cpp b/src/control/trajectory_point_interface.cpp
@@ -59,9 +59,7 @@ TrajectoryPointInterface::TrajectoryPointInterface(uint32_t port) : ReverseInter
 {
 }
 
-bool TrajectoryPointInterface::writeTrajectoryPoint(const vector6d_t* positions, const float acceleration,
-                                                    const float velocity, const float goal_time,
-                                                    const float blend_radius, const bool cartesian)
+bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptr<control::MotionPrimitive> primitive)
 {
   if (client_fd_ == -1)
   {
@@ -70,48 +68,62 @@ bool TrajectoryPointInterface::writeTrajectoryPoint(const vector6d_t* positions,
   uint8_t buffer[sizeof(int32_t) * MESSAGE_LENGTH];
   uint8_t* b_pos = buffer;
 
-  if (positions != nullptr)
+  vector6d_t positions;
+
+  switch (primitive->type)
   {
-    for (auto const& pos : *positions)
+    case MotionType::MOVEJ:
     {
-      int32_t val = static_cast<int32_t>(round(pos * MULT_JOINTSTATE));
-      val = htobe32(val);
-      b_pos += append(b_pos, val);
+      auto movej_primitive = std::static_pointer_cast<control::MoveJPrimitive>(primitive);
+      positions = movej_primitive->target_joint_configuration;
+      break;
     }
-    for (size_t i = 0; i < positions->size(); ++i)
+    case MotionType::MOVEL:
     {
-      int32_t val = static_cast<int32_t>(round(velocity * MULT_JOINTSTATE));
-      val = htobe32(val);
-      b_pos += append(b_pos, val);
+      auto movel_primitive = std::static_pointer_cast<control::MoveLPrimitive>(primitive);
+      positions = { movel_primitive->target_pose.x,  movel_primitive->target_pose.y,  movel_primitive->target_pose.z,
+                    movel_primitive->target_pose.rx, movel_primitive->target_pose.ry, movel_primitive->target_pose.rz };
+      break;
     }
-    for (size_t i = 0; i < positions->size(); ++i)
+    case urcl::control::MotionType::MOVEP:
     {
-      int32_t val = static_cast<int32_t>(round(acceleration * MULT_JOINTSTATE));
-      val = htobe32(val);
-      b_pos += append(b_pos, val);
+      auto movep_primitive = std::static_pointer_cast<control::MovePPrimitive>(primitive);
+      positions = { movep_primitive->target_pose.x,  movep_primitive->target_pose.y,  movep_primitive->target_pose.z,
+                    movep_primitive->target_pose.rx, movep_primitive->target_pose.ry, movep_primitive->target_pose.rz };
+      break;
     }
+    default:
+      throw UnsupportedMotionType();
   }
-  else
+
+  for (auto const& pos : positions)
   {
-    b_pos += 6 * sizeof(int32_t);
+    int32_t val = static_cast<int32_t>(round(pos * MULT_JOINTSTATE));
+    val = htobe32(val);
+    b_pos += append(b_pos, val);
+  }
+  for (size_t i = 0; i < positions.size(); ++i)
+  {
+    int32_t val = static_cast<int32_t>(round(primitive->velocity * MULT_JOINTSTATE));
+    val = htobe32(val);
+    b_pos += append(b_pos, val);
+  }
+  for (size_t i = 0; i < positions.size(); ++i)
+  {
+    int32_t val = static_cast<int32_t>(round(primitive->acceleration * MULT_JOINTSTATE));
+    val = htobe32(val);
+    b_pos += append(b_pos, val);
   }
 
-  int32_t val = static_cast<int32_t>(round(goal_time * MULT_TIME));
+  int32_t val = static_cast<int32_t>(round(primitive->duration.count() * MULT_TIME));
   val = htobe32(val);
   b_pos += append(b_pos, val);
 
-  val = static_cast<int32_t>(round(blend_radius * MULT_TIME));
+  val = static_cast<int32_t>(round(primitive->blend_radius * MULT_TIME));
   val = htobe32(val);
   b_pos += append(b_pos, val);
 
-  if (cartesian)
-  {
-    val = static_cast<int32_t>(control::MotionType::MOVEL);
-  }
-  else
-  {
-    val = static_cast<int32_t>(control::MotionType::MOVEJ);
-  }
+  val = static_cast<int32_t>(primitive->type);
 
   val = htobe32(val);
   b_pos += append(b_pos, val);
@@ -121,6 +133,28 @@ bool TrajectoryPointInterface::writeTrajectoryPoint(const vector6d_t* positions,
   return server_.write(client_fd_, buffer, sizeof(buffer), written);
 }
 
+bool TrajectoryPointInterface::writeTrajectoryPoint(const vector6d_t* positions, const float acceleration,
+                                                    const float velocity, const float goal_time,
+                                                    const float blend_radius, const bool cartesian)
+{
+  std::shared_ptr<MotionPrimitive> primitive;
+  if (cartesian)
+  {
+    primitive = std::make_shared<MoveLPrimitive>(
+        urcl::Pose{ (*positions)[0], (*positions)[1], (*positions)[2], (*positions)[3], (*positions)[4],
+                    (*positions)[5] },
+        blend_radius, std::chrono::milliseconds(static_cast<int>(goal_time * 1000)), acceleration, velocity);
+  }
+  else
+  {
+    primitive = std::make_shared<MoveJPrimitive>(*positions, blend_radius,
+                                                 std::chrono::milliseconds(static_cast<int>(goal_time * 1000)),
+                                                 acceleration, velocity);
+  }
+
+  return writeMotionPrimitive(primitive);
+}
+
 bool TrajectoryPointInterface::writeTrajectoryPoint(const vector6d_t* positions, const float goal_time,
                                                     const float blend_radius, const bool cartesian)
 {
@@ -249,4 +283,4 @@ void TrajectoryPointInterface::messageCallback(const socket_t filedescriptor, ch
   }
 }
 }  // namespace control
-}  // namespace urcl
+}  // namespace urcl
diff --git a/src/ur/instruction_executor.cpp b/src/ur/instruction_executor.cpp
@@ -85,6 +85,16 @@ bool urcl::InstructionExecutor::executeMotion(
                                       primitive->duration.count(), primitive->blend_radius);
         break;
       }
+      case control::MotionType::MOVEP:
+      {
+        auto movep_primitive = std::static_pointer_cast<control::MovePPrimitive>(primitive);
+        urcl::vector6d_t pose_vec = { movep_primitive->target_pose.x,  movep_primitive->target_pose.y,
+                                      movep_primitive->target_pose.z,  movep_primitive->target_pose.rx,
+                                      movep_primitive->target_pose.ry, movep_primitive->target_pose.rz };
+        driver_->writeTrajectoryPoint(pose_vec, primitive->acceleration, primitive->velocity, false,
+                                      primitive->duration.count(), primitive->blend_radius);
+        break;
+      }
       default:
         URCL_LOG_ERROR("Unsupported motion type");
         // The hardware will complain about missing trajectory points and return a failure for
diff --git a/tests/test_trajectory_point_interface.cpp b/tests/test_trajectory_point_interface.cpp
