Implemented spline interpolation in joint space

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;
   /*!

include/ur_client_library/control/trajectory_point_interface.h

@@ -59,6 +59,7 @@ class TrajectoryPointInterface : public ReverseInterface
   static const int32_t MULT_TIME = 1000;
   static const int32_t JOINT_POINT = 0;
   static const int32_t CARTESIAN_POINT = 1;
+  static const int32_t JOINT_POINT_SPLINE = 2;
 
   TrajectoryPointInterface() = delete;
   /*!
@@ -86,6 +87,20 @@ class TrajectoryPointInterface : public ReverseInterface
   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
+   * velocity and acceleration 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 accelerations A vector of joint or cartesian target accelerations for the robot
+   * \param goal_time The goal time to reach the target
+   *
+   * \returns True, if the write was performed successfully, false otherwise.
+   */
+  bool writeTrajectorySplinePoint(const vector6d_t* positions, const vector6d_t* velocities,
+                                  const vector6d_t* accelerations, const float goal_time);
+
   void setTrajectoryEndCallback(std::function<void(TrajectoryResult)> callback)
   {
     handle_trajectory_end_ = callback;
@@ -99,6 +114,7 @@ class TrajectoryPointInterface : public ReverseInterface
   virtual void messageCallback(const int filedescriptor, char* buffer, int nbytesrecv) override;
 
 private:
+  static const int MESSAGE_LENGTH = 21;
   std::function<void(TrajectoryResult)> handle_trajectory_end_;
 };
 

include/ur_client_library/ur/ur_driver.h

@@ -201,16 +201,51 @@ class UrDriver
   /*!
    * \brief Writes a trajectory point onto the dedicated socket.
    *
-   * \param values Desired joint or cartesian positions
+   * \param positions Desired joint or cartesian positions
    * \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& values, const bool cartesian, const float goal_time = 0.0,
+  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 spline point onto the dedicated socket.
+   *
+   * \param positions Desired joint positions
+   * \param velocities Desired joint velocities
+   * \param accelerations Desired joint accelerations
+   * \param goal_time Time for the robot to reach this point
+   *
+   * \returns True on successful write.
+   */
+  bool writeTrajectorySplinePoint(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 positions
+   * \param velocities Desired joint velocities
+   * \param goal_time Time for the robot to reach this point
+   *
+   * \returns True on successful write.
+   */
+  bool writeTrajectorySplinePoint(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 positions
+   * \param goal_time Time for the robot to reach this point
+   *
+   * \returns True on successful write.
+   */
+  bool writeTrajectorySplinePoint(const vector6d_t& positions, const float goal_time = 0.0);
+
   /*!
    * \brief Writes a control message in trajectory forward mode.
    *