Add functionality to send MoveP and MoveC instructions to the robot (#303)

This PR aims at supporting to send MoveP and MoveC commands to the robot
directly through the TrajectoryPointInterface and the
InstructionExecutor.

Author: urfeex

doc/architecture/instruction_executor.rst

@@ -6,9 +6,11 @@ Instruction Executor
 The Instruction Executor is a convenience wrapper to make common robot instructions such as point
 to point motions easily accessible. Currently, it supports the following instructions:
 
-* Excecute MoveJ point to point motions
+* Execute MoveJ point to point motions
 * Execute MoveL point to point motions
-* Execute sequences consisting of MoveJ and MoveL instructions
+* Execute MoveP point to point motions
+* Execute MoveC circular motions
+* Execute sequences consisting of the motion primitives above
 
 The Instruction Executor uses the :ref:`trajectory_point_interface` and the
 :ref:`reverse_interface`

doc/architecture/trajectory_point_interface.rst

@@ -39,13 +39,32 @@ representations in 21 datafields. The data fields have the following meaning:
    =====  =====
    index  meaning
    =====  =====
-   0-5    trajectory point positions (floating point)
-   6-11   trajectory point velocities (floating point)
-   12-17  trajectory point accelerations (floating point)
-   18     trajectory point type (0: JOINT, 1: CARTESIAN, 2: JOINT_SPLINE)
-   19     trajectory point time (in seconds, floating point)
+   0-5    trajectory point positions (Multiplied by ``MULT_JOINTSTATE``)
+   6-11   trajectory point velocities (Multiplied by ``MULT_JOINTSTATE``). For MOVEC, this contains the "via pose".
+   12-17  trajectory point accelerations (Multiplied by ``MULT_JOINTSTATE``).
+
+          For MOVEC:
+
+          - 12: velocity (Multiplied by ``MULT_JOINTSTATE``)
+          - 13: acceleration (Multiplied by ``MULT_JOINTSTATE``)
+          - 14: mode (Multiplied by ``MULT_JOINTSTATE``)
+
+   18     trajectory point type
+
+          - 0: MOVEJ
+          - 1: MOVEL
+          - 2: MOVEP
+          - 3: MOVEC
+          - 51: SPLINE)
+
+   19     trajectory point time (in seconds, multiplied by ``MULT_TIME``)
    20     depending on trajectory point type
 
-          - JOINT, CARTESIAN: point blend radius (in meters, floating point)
-          - JOINT_SPLINE: spline type (1: CUBIC, 2: QUINTIC)
+          - MOVEJ, MOVEL, MOVEP and MOVEC: point blend radius (in meters, multiplied by ``MULT_TIME``)
+          - SPLINE: spline type (1: CUBIC, 2: QUINTIC)
    =====  =====
+
+where
+
+- ``MULT_JOINTSTATE``: 1000000
+- ``MULT_TIME``: 1000

doc/examples/instruction_executor.rst

@@ -44,10 +44,12 @@ 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 and two ``MoveL`` motion. The primitives' parameters are directly forwarded to
+``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. The latter will be ignored if a time > 0 is given.
+/ 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.
 
 Execute a single motion
 -----------------------

examples/instruction_executor.cpp

@@ -89,8 +89,8 @@ int main(int argc, char* argv[])
 
     std::make_shared<urcl::control::MoveLPrimitive>(urcl::Pose(-0.203, 0.263, 0.559, 0.68, -1.083, -2.076), 0.1,
                                                     std::chrono::seconds(2)),
-    std::make_shared<urcl::control::MoveLPrimitive>(urcl::Pose{ -0.203, 0.463, 0.559, 0.68, -1.083, -2.076 }, 0.1,
-                                                    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),
   };
   instruction_executor->executeMotion(motion_sequence);
 
@@ -105,6 +105,8 @@ int main(int argc, char* argv[])
   // goal time parametrization -- acceleration and velocity will be ignored
   instruction_executor->moveL({ -0.203, 0.463, 0.559, 0.68, -1.083, -2.076 }, 0.1, 0.1, goal_time_sec);
 
+  instruction_executor->moveP({ -0.203, 0.463, 0.759, 0.68, -1.083, -2.076 }, 1.5, 1.5);
+
   g_my_robot->getUrDriver()->stopControl();
   return 0;
 }

include/ur_client_library/control/motion_primitives.h

@@ -32,6 +32,7 @@
 #define UR_CLIENT_LIBRARY_MOTION_PRIMITIVES_H_INCLUDED
 
 #include <chrono>
+#include <optional>
 #include <ur_client_library/types.h>
 
 namespace urcl
@@ -48,9 +49,19 @@ enum class MotionType : uint8_t
   SPLINE = 51,
   UNKNOWN = 255
 };
+
+/*!
+ * Spline types
+ */
+enum class TrajectorySplineType : int32_t
+{
+  SPLINE_CUBIC = 1,
+  SPLINE_QUINTIC = 2
+};
+
 struct MotionPrimitive
 {
-  MotionType type;
+  MotionType type = MotionType::UNKNOWN;
   std::chrono::duration<double> duration;
   double acceleration;
   double velocity;
@@ -90,6 +101,67 @@ 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, const int32_t mode = 0)
+  {
+    type = MotionType::MOVEC;
+    via_point_pose = via_point;
+    target_pose = target;
+    this->acceleration = acceleration;
+    this->velocity = velocity;
+    this->blend_radius = blend_radius;
+    this->mode = mode;
+  }
+  urcl::Pose via_point_pose;
+  urcl::Pose target_pose;
+  int32_t mode = 0;
+};
+
+struct SplinePrimitive : public MotionPrimitive
+{
+  SplinePrimitive(const urcl::vector6d_t& target_positions, const vector6d_t& target_velocities,
+                  const std::optional<vector6d_t>& target_accelerations,
+                  const std::chrono::duration<double> duration = std::chrono::milliseconds(0))
+  {
+    type = MotionType::SPLINE;
+    this->target_positions = target_positions;
+    this->target_velocities = target_velocities;
+    this->target_accelerations = target_accelerations;
+    this->duration = duration;
+  }
+
+  control::TrajectorySplineType getSplineType() const
+  {
+    if (target_accelerations.has_value())
+    {
+      return control::TrajectorySplineType::SPLINE_QUINTIC;
+    }
+    else
+    {
+      return control::TrajectorySplineType::SPLINE_CUBIC;
+    }
+  }
+  vector6d_t target_positions;
+  vector6d_t target_velocities;
+  std::optional<vector6d_t> target_accelerations;
+};
 }  // namespace control
 }  // namespace urcl
-#endif  // UR_CLIENT_LIBRARY_MOTION_PRIMITIVES_H_INCLUDED
+#endif  // UR_CLIENT_LIBRARY_MOTION_PRIMITIVES_H_INCLUDED

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"
@@ -54,25 +55,6 @@ enum class TrajectoryResult : int32_t
 
 std::string trajectoryResultToString(const TrajectoryResult result);
 
-/*!
- * Spline types
- */
-enum class TrajectorySplineType : int32_t
-{
-  SPLINE_CUBIC = 1,
-  SPLINE_QUINTIC = 2
-};
-
-/*!
- * Motion Types
- */
-enum class TrajectoryMotionType : int32_t
-{
-  JOINT_POINT = 0,
-  CARTESIAN_POINT = 1,
-  JOINT_POINT_SPLINE = 2
-};
-
 /*!
  * \brief The TrajectoryPointInterface class handles trajectory forwarding to the robot. Full
  * trajectories are forwarded to the robot controller and are executed there.
@@ -141,6 +123,16 @@ class TrajectoryPointInterface : public ReverseInterface
   bool writeTrajectorySplinePoint(const vector6d_t* positions, const vector6d_t* velocities,
                                   const vector6d_t* accelerations, const float goal_time);
 
+  /*!
+   * \brief Writes a motion primitive to the robot to be read by the URScript program.
+   *
+   * \param primitive A shared pointer to a motion primitive object. This can contain any motion
+   * primitive type that is supported. Currently, that is MoveJ, MoveL, MoveP and MoveC.
+   *
+   * \returns True, if the write was performed successfully, false otherwise.
+   */
+  bool writeMotionPrimitive(const std::shared_ptr<control::MotionPrimitive> primitive);
+
   void setTrajectoryEndCallback(std::function<void(TrajectoryResult)> callback)
   {
     handle_trajectory_end_ = callback;

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

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>

include/ur_client_library/ur/instruction_executor.h

@@ -82,10 +82,10 @@ class InstructionExecutor
              const double time = 0, const double blend_radius = 0);
 
   /**
-   * \brief Move the robot to a pose target.
+   * \brief Move the robot to a pose target using movel
    *
    * This function will move the robot to the given pose target. The robot will move with the given acceleration and
-   * velocity. The function will return once the robot has reached the target.
+   * velocity or a motion time. The function will return once the robot has reached the target.
    *
    * \param target The pose target to move to.
    * \param acceleration Tool acceleration [m/s^2]
@@ -98,6 +98,40 @@ 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);
 
+  /**
+   * \brief Move the robot to a pose target using movep
+   *
+   * This function will move the robot to the given pose target. The robot will move with the given acceleration and
+   * velocity. The function will return once the robot has reached the target.
+   *
+   * \param target The pose target to move to.
+   * \param acceleration Tool acceleration [m/s^2]
+   * \param velocity Tool speed [m/s]
+   * \param blend_radius The blend radius to use for the motion.
+   *
+   * \return True if the robot has reached the target, false otherwise.
+   */
+  bool moveP(const urcl::Pose& target, const double acceleration = 1.4, const double velocity = 1.04,
+             const double blend_radius = 0.0);
+
+  /**
+   * \brief Move the robot to a pose target using movec
+   *
+   * This function will move the robot to the given pose target in a circular motion going through via. The robot will
+   * move with the given acceleration and velocity. The function will return once the robot has reached the target.
+   *
+   * \param via The circle will be defined by the current pose (the end pose of the previous motion), the target and the
+   * via point.
+   * \param target The pose target to move to.
+   * \param acceleration Tool acceleration [m/s^2]
+   * \param velocity Tool speed [m/s]
+   * \param blend_radius The blend radius to use for the motion.
+   *
+   * \return True if the robot has reached the target, false otherwise.
+   */
+  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 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,18 @@ class UrDriver
    */
   bool writeTrajectorySplinePoint(const vector6d_t& positions, const float goal_time = 0.0);
 
+  /*!
+   * \brief Writes a motion command to the trajectory point interface
+   *
+   * The motion command corresponds directly to a URScript move function such as `movej` or
+   * `movel`. See the MotionPrimitive's header for all possible motion primitives.
+   *
+   * \param motion_instruction The motion primitive to be sent to the robot.
+   *
+   * \returns True on successful write.
+   */
+  bool writeMotionPrimitive(const std::shared_ptr<control::MotionPrimitive> motion_instruction);
+
   /*!
    * \brief Writes a control message in trajectory forward mode.
    *