Implemented spline interpolation in joint space in URScript

UniversalRobots · Jun 9, 2022 · 688fa0b · 688fa0b
1 parent e50031e
commit 688fa0b
Show file tree

Hide file tree

Showing 7 changed files with 274 additions and 136 deletions.
diff --git a/include/ur_client_library/control/reverse_interface.h b/include/ur_client_library/control/reverse_interface.h
@@ -59,6 +59,8 @@ class ReverseInterface
 {
 public:
   static const int32_t MULT_JOINTSTATE = 1000000;
+  static const int32_t POINT_INTERPOLATION = 0;
+  static const int32_t SPLINE_INTERPOLATION = 1;
 
   ReverseInterface() = delete;
   /*!
@@ -90,10 +92,12 @@ class ReverseInterface
    *
    * \param trajectory_action 1 if a trajectory is to be started, -1 if it should be stopped
    * \param point_number The number of points of the trajectory to be executed
+   * \param spline_interpolation True, if spline interpolation should be used, false if not
    *
    * \returns True, if the write was performed successfully, false otherwise.
    */
-  bool writeTrajectoryControlMessage(const TrajectoryControlMessage trajectory_action, const int point_number = 0);
+  bool writeTrajectoryControlMessage(const TrajectoryControlMessage trajectory_action, const int point_number = 0,
+                                     const bool spline_interpolation = false);
 
   /*!
    * \brief Set the Keepalive count. This will set the number of allowed timeout reads on the robot.

diff --git a/include/ur_client_library/control/trajectory_point_interface.h b/include/ur_client_library/control/trajectory_point_interface.h
@@ -56,13 +56,9 @@ enum class TrajectoryResult : int32_t
 class TrajectoryPointInterface : public ReverseInterface
 {
 public:
-  enum class PointType : int32_t
-  {
-    JOINT_POINT = 0,
-    CARTESIAN_POINT = 1,
-    JOINT_SPLINE = 2
-  };
   static const int32_t MULT_TIME = 1000;
+  static const int32_t JOINT_POINT = 0;
+  static const int32_t CARTESIAN_POINT = 1;
 
   TrajectoryPointInterface() = delete;
   /*!
@@ -83,12 +79,12 @@ class TrajectoryPointInterface : public ReverseInterface
    * \param positions A vector of joint or cartesian targets for the robot
    * \param goal_time The goal time to reach the target
    * \param blend_radius The radius to be used for blending between control points
-   * \param type The type used for the trajectory point
+   * \param cartesian True, if the written point is specified in cartesian space, false if in joint space
    *
    * \returns True, if the write was performed successfully, false otherwise.
    */
-  bool writeTrajectoryPoint(vector6d_t const* positions, const float goal_time, const float blend_radius,
-                            const PointType type);
+  bool writeTrajectoryPoint(const vector6d_t* positions, const float goal_time, const float blend_radius,
+                            const bool cartesian);
 
   /*!
    * \brief Writes needed information to the robot to be read by the URScript program including
@@ -97,14 +93,34 @@ class TrajectoryPointInterface : public ReverseInterface
    * \param positions A vector of joint or cartesian target positions for the robot
    * \param velocities A vector of joint or cartesian target velocities for the robot
    * \param accelerations A vector of joint or cartesian target accelerations for the robot
-   * \param time The goal time to reach the target
-   * \param blend_radius The radius to be used for blending between control points
-   * \param type The type used for the trajectory point
+   * \param goal_time The goal time to reach the target
+   *
+   * \returns True, if the write was performed successfully, false otherwise.
+   */
+  bool writeSplinePoint(const vector6d_t* positions, const vector6d_t* velocities, const vector6d_t* accelerations,
+                        const float goal_time);
+
+  /*!
+   * \brief Writes needed information to the robot to be read by the URScript program including
+   * velocity information.
+   *
+   * \param positions A vector of joint or cartesian target positions for the robot
+   * \param velocities A vector of joint or cartesian target velocities for the robot
+   * \param goal_time The goal time to reach the target
+   *
+   * \returns True, if the write was performed successfully, false otherwise.
+   */
+  bool writeSplinePoint(const vector6d_t* positions, const vector6d_t* velocities, const float goal_time);
+
+  /*!
+   * \brief Writes needed information to the robot to be read by the URScript program.
+   *
+   * \param positions A vector of joint or cartesian target positions for the robot
+   * \param goal_time The goal time to reach the target
    *
    * \returns True, if the write was performed successfully, false otherwise.
    */
-  bool writeTrajectoryPoint(vector6d_t const* positions, vector6d_t const* velocities, vector6d_t const* accelerations,
-                            const float goal_time, const float blend_radius, const PointType type);
+  bool writeSplinePoint(const vector6d_t* positions, const float goal_time);
 
   void setTrajectoryEndCallback(std::function<void(TrajectoryResult)> callback)
   {

diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h
@@ -193,41 +193,60 @@ class UrDriver
    * \brief Writes a trajectory point onto the dedicated socket.
    *
    * \param positions Desired joint or cartesian positions
-   * \param type The type used for the trajectory point
+   * \param cartesian True, if the point sent is cartesian, false if joint-based
    * \param goal_time Time for the robot to reach this point
    * \param blend_radius  The radius to be used for blending between control points
    *
    * \returns True on successful write.
    */
-  bool writeTrajectoryPoint(const vector6d_t& positions, const control::TrajectoryPointInterface::PointType type,
-                            const float goal_time = 0.0, const float blend_radius = 0.052);
+  bool writeTrajectoryPoint(const vector6d_t& positions, const bool cartesian, const float goal_time = 0.0,
+                            const float blend_radius = 0.052);
 
   /*!
-   * \brief Writes a trajectory point onto the dedicated socket.
+   * \brief Writes a trajectory spline point onto the dedicated socket.
    *
    * \param positions Desired joint or cartesian positions
    * \param velocities Desired joint or cartesian velocities
    * \param accelerations Desired joint or cartesian accelerations
-   * \param type The type used for the trajectory point
    * \param goal_time Time for the robot to reach this point
-   * \param blend_radius  The radius to be used for blending between control points
    *
    * \returns True on successful write.
    */
-  bool writeTrajectoryPoint(const vector6d_t& positions, const vector6d_t& velocities, const vector6d_t& accelerations,
-                            const control::TrajectoryPointInterface::PointType type, const float goal_time = 0.0,
-                            const float blend_radius = 0.052);
+  bool writeSplinePoint(const vector6d_t& positions, const vector6d_t& velocities, const vector6d_t& accelerations,
+                        const float goal_time = 0.0);
+
+  /*!
+   * \brief Writes a trajectory spline point onto the dedicated socket.
+   *
+   * \param positions Desired joint or cartesian positions
+   * \param velocities Desired joint or cartesian velocities
+   * \param goal_time Time for the robot to reach this point
+   *
+   * \returns True on successful write.
+   */
+  bool writeSplinePoint(const vector6d_t& positions, const vector6d_t& velocities, const float goal_time = 0.0);
+
+  /*!
+   * \brief Writes a trajectory spline point onto the dedicated socket.
+   *
+   * \param positions Desired joint or cartesian positions
+   * \param goal_time Time for the robot to reach this point
+   *
+   * \returns True on successful write.
+   */
+  bool writeSplinePoint(const vector6d_t& positions, const float goal_time = 0.0);
 
   /*!
    * \brief Writes a control message in trajectory forward mode.
    *
    * \param trajectory_action The action to be taken, such as starting a new trajectory
    * \param point_number The number of points of a new trajectory to be sent
+   * \param spline_interpolation True, if spline interpolation should be used, false if not
    *
    * \returns True on successful write.
    */
   bool writeTrajectoryControlMessage(const control::TrajectoryControlMessage trajectory_action,
-                                     const int point_number = 0);
+                                     const int point_number = 0, const bool spline_interpolation = false);
 
   /*!
    * \brief Write a keepalive signal only.