Add moveP to instruction executor

Author: urfeex

include/ur_client_library/ur/instruction_executor.h

@@ -98,6 +98,9 @@ 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.0);
+
   /**
    * \brief Cancel the current motion.
    *

include/ur_client_library/ur/ur_driver.h

@@ -30,6 +30,7 @@
 
 #include <chrono>
 #include <functional>
+#include <memory>
 
 #include "ur_client_library/rtde/rtde_client.h"
 #include "ur_client_library/control/reverse_interface.h"
@@ -531,6 +532,8 @@ class UrDriver
    */
   bool writeTrajectorySplinePoint(const vector6d_t& positions, const float goal_time = 0.0);
 
+  bool writeMotionPrimitive(const std::shared_ptr<control::MotionPrimitive> motion_instruction);
+
   /*!
    * \brief Writes a control message in trajectory forward mode.
    *

src/ur/instruction_executor.cpp

@@ -66,29 +66,15 @@ bool urcl::InstructionExecutor::executeMotion(
 
   for (const auto& primitive : motion_sequence)
   {
-    switch (primitive->type)
+    try
     {
-      case control::MotionType::MOVEJ:
-      {
-        auto movej_primitive = std::static_pointer_cast<control::MoveJPrimitive>(primitive);
-        driver_->writeTrajectoryPoint(movej_primitive->target_joint_configuration, primitive->acceleration,
-                                      primitive->velocity, false, primitive->duration.count(), primitive->blend_radius);
-        break;
-      }
-      case control::MotionType::MOVEL:
-      {
-        auto movel_primitive = std::static_pointer_cast<control::MoveLPrimitive>(primitive);
-        urcl::vector6d_t pose_vec = { 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 };
-        driver_->writeTrajectoryPoint(pose_vec, primitive->acceleration, primitive->velocity, true,
-                                      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
-        // trajectory execution. Hence, we need to step into the running loop below.
+      driver_->writeMotionPrimitive(primitive);
+    }
+    catch (const UnsupportedMotionType&)
+    {
+      URCL_LOG_ERROR("Unsupported motion type");
+      // The hardware will complain about missing trajectory points and return a failure for
+      // trajectory execution. Hence, we need to step into the running loop below.
     }
   }
   trajectory_running_ = true;
@@ -120,6 +106,12 @@ bool urcl::InstructionExecutor::moveL(const urcl::Pose& target, const double acc
       target, blend_radius, std::chrono::milliseconds(static_cast<int>(time * 1000)), acceleration, velocity) });
 }
 
+bool urcl::InstructionExecutor::moveP(const urcl::Pose& target, const double acceleration, const double velocity,
+                                      const double blend_radius)
+{
+  return executeMotion({ std::make_shared<control::MovePPrimitive>(target, blend_radius, acceleration, velocity) });
+}
+
 bool urcl::InstructionExecutor::cancelMotion()
 {
   cancel_requested_ = true;

src/ur/ur_driver.cpp

@@ -240,6 +240,11 @@ bool UrDriver::writeTrajectoryControlMessage(const control::TrajectoryControlMes
   return reverse_interface_->writeTrajectoryControlMessage(trajectory_action, point_number, robot_receive_timeout);
 }
 
+bool UrDriver::writeMotionPrimitive(const std::shared_ptr<control::MotionPrimitive> motion_instruction)
+{
+  return trajectory_interface_->writeMotionPrimitive(motion_instruction);
+}
+
 bool UrDriver::writeFreedriveControlMessage(const control::FreedriveControlMessage freedrive_action,
                                             const RobotReceiveTimeout& robot_receive_timeout)
 {

tests/test_instruction_executor.cpp

@@ -363,6 +363,14 @@ TEST_F(InstructionExecutorTest, empty_sequence_succeeds)
   ASSERT_TRUE(executor_->executeMotion(motion_sequence));
 }
 
+TEST_F(InstructionExecutorTest, movep_succeeds)
+{
+  // move to a feasible starting pose
+  ASSERT_TRUE(executor_->moveJ({ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 }));
+
+  ASSERT_TRUE(executor_->moveP({ -0.203, 0.263, 0.559, 0.68, -1.083, -2.076 }, 2.0, 2.0));
+}
+
 int main(int argc, char* argv[])
 {
   ::testing::InitGoogleTest(&argc, argv);