diff --git a/CMakeLists.txt b/CMakeLists.txt
index 634fe8156..6717e8e56 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.14.0)
 project(ur_client_library)
 
 set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/CMakeModules/" ${CMAKE_MODULE_PATH})
@@ -15,12 +15,6 @@ if(MSVC)
 set(BUILD_SHARED_LIBS OFF)
 endif()
 
-##
-## Check C++11 support / enable global pedantic and Wall
-##
-include(DefineCXX17CompilerFlag)
-DEFINE_CXX_17_COMPILER_FLAG(CXX17_FLAG)
-
 add_library(urcl
     src/comm/tcp_socket.cpp
     src/comm/tcp_server.cpp
@@ -55,14 +49,13 @@ add_library(urcl
     src/helpers.cpp
 )
 add_library(ur_client_library::urcl ALIAS urcl)
+target_compile_features(urcl PUBLIC cxx_std_17)
 
 if(MSVC)
   include_directories(${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/endian)
   target_link_libraries(urcl ws2_32)
 else()
-  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -pedantic")
   target_compile_options(urcl PRIVATE -Wall -Wextra -Wno-unused-parameter)
-
   if(WITH_ASAN)
     target_compile_options(urcl PUBLIC -fsanitize=address)
     target_link_options(urcl PUBLIC -fsanitize=address)
diff --git a/CMakeModules/DefineCXX11CompilerFlag.cmake b/CMakeModules/DefineCXX11CompilerFlag.cmake
deleted file mode 100644
index f362e364a..000000000
--- a/CMakeModules/DefineCXX11CompilerFlag.cmake
+++ /dev/null
@@ -1,55 +0,0 @@
-# DefineCXX11CompilerFlag
-# -----------------------
-#
-# Tries to find the compiler flag which can be used to enable C++11 on the current compiler.
-# If the flag is not found, the macro will issue an cmake error.
-#
-# DEFINE_CXX_11_COMPILER_FLAG(<var>)
-#
-# ::
-#
-#   <var>  - variable to store the resulting flag
-#
-# Use this to find the compiler option to enable C++11 compilation. This is usefull
-# on cmake versions / use cases which do not support CXX_STANDARD.
-# Use this in conjunction with target_compiler_option():
-#
-# include(DefineCXX11CompilerFlag)
-# DEFINE_CXX_11_COMPILER_FLAG(CXX11_FLAG)
-# ...
-# target_compile_options(topt PUBLIC ${CXX11_FLAG})
-# ...
-#
-# The following variables may be set before calling this macro to modify
-# the way the check is run:
-#
-# ::
-#
-#   CMAKE_REQUIRED_QUIET = execute quietly without messages
-#
-
-include(CheckCXXCompilerFlag)
-
-macro (DEFINE_CXX_11_COMPILER_FLAG _RESULT)
-  if(NOT DEFINED "${_RESULT}")
-
-    if(NOT CMAKE_REQUIRED_QUIET)
-      message(STATUS "Performing C++11 Test")
-    endif()
-
-    # Check for default argument (all newer compilers)
-    CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
-    if(COMPILER_SUPPORTS_CXX11)
-      set(${_RESULT} "-std=c++11" CACHE INTERNAL "C++11 flag")
-    else()
-      # Check for older version (before 2011)
-      CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
-      if(COMPILER_SUPPORTS_CXX0X)
-	set(${_RESULT} "-std=c++0x" CACHE INTERNAL "C++11 flag")
-      else()
-	message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
-      endif()
-    endif()
-
-  endif()
-endmacro()
diff --git a/CMakeModules/DefineCXX14CompilerFlag.cmake b/CMakeModules/DefineCXX14CompilerFlag.cmake
deleted file mode 100644
index c76f2caea..000000000
--- a/CMakeModules/DefineCXX14CompilerFlag.cmake
+++ /dev/null
@@ -1,55 +0,0 @@
-# DefineCXX14CompilerFlag
-# -----------------------
-#
-# Tries to find the compiler flag which can be used to enable C++14 on the current compiler.
-# If the flag is not found, the macro will issue an cmake error.
-#
-# DEFINE_CXX_14_COMPILER_FLAG(<var>)
-#
-# ::
-#
-#   <var>  - variable to store the resulting flag
-#
-# Use this to find the compiler option to enable C++14 compilation. This is usefull
-# on cmake versions / use cases which do not support CXX_STANDARD.
-# Use this in conjunction with target_compiler_option():
-#
-# include(DefineCXX14CompilerFlag)
-# DEFINE_CXX_14_COMPILER_FLAG(CXX14_FLAG)
-# ...
-# target_compile_options(topt PUBLIC ${CXX14_FLAG})
-# ...
-#
-# The following variables may be set before calling this macro to modify
-# the way the check is run:
-#
-# ::
-#
-#   CMAKE_REQUIRED_QUIET = execute quietly without messages
-#
-
-include(CheckCXXCompilerFlag)
-
-macro (DEFINE_CXX_14_COMPILER_FLAG _RESULT)
-  if(NOT DEFINED "${_RESULT}")
-
-    if(NOT CMAKE_REQUIRED_QUIET)
-      message(STATUS "Performing C++14 Test")
-    endif()
-
-    # Check for default argument (all newer compilers)
-    CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
-    if(COMPILER_SUPPORTS_CXX14)
-      set(${_RESULT} "-std=c++14" CACHE INTERNAL "C++14 flag")
-    else()
-      # Check for older version (like gcc-4.8.4)
-      CHECK_CXX_COMPILER_FLAG("-std=c++1y" COMPILER_SUPPORTS_CXX1Y)
-      if(COMPILER_SUPPORTS_CXX1Y)
-	set(${_RESULT} "-std=c++1y" CACHE INTERNAL "C++14 flag")
-      else()
-	message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler.")
-      endif()
-    endif()
-
-  endif()
-endmacro()
diff --git a/CMakeModules/DefineCXX17CompilerFlag.cmake b/CMakeModules/DefineCXX17CompilerFlag.cmake
deleted file mode 100644
index ad9992e65..000000000
--- a/CMakeModules/DefineCXX17CompilerFlag.cmake
+++ /dev/null
@@ -1,58 +0,0 @@
-# DefineCXX17CompilerFlag
-# -----------------------
-#
-# Tries to find the compiler flag which can be used to enable C++17 on the current compiler.
-# If the flag is not found, the macro will issue an cmake error.
-#
-# DEFINE_CXX_17_COMPILER_FLAG(<var>)
-#
-# ::
-#
-#   <var>  - variable to store the resulting flag
-#
-# Use this to find the compiler option to enable C++17 compilation. This is usefull
-# on cmake versions / use cases which do not support CXX_STANDARD.
-# Use this in conjunction with target_compiler_option():
-#
-# include(DefineCXX17CompilerFlag)
-# DEFINE_CXX_17_COMPILER_FLAG(CXX17_FLAG)
-# ...
-# target_compile_options(topt PUBLIC ${CXX17_FLAG})
-# ...
-#
-# The following variables may be set before calling this macro to modify
-# the way the check is run:
-#
-# ::
-#
-#   CMAKE_REQUIRED_QUIET = execute quietly without messages
-#
-
-include(CheckCXXCompilerFlag)
-
-macro (DEFINE_CXX_17_COMPILER_FLAG _RESULT)
-  if(NOT DEFINED "${_RESULT}")
-
-    if(MSVC AND MSVC_VERSION GREATER 1919)
-      set(${_RESULT} "/std:c++17" CACHE INTERNAL "C++17 flag")
-    else()
-      if(NOT CMAKE_REQUIRED_QUIET)
-        message(STATUS "Performing C++17 Test")
-      endif()
-
-      # Check for default argument (all newer compilers)
-      CHECK_CXX_COMPILER_FLAG("-std=c++17" COMPILER_SUPPORTS_CXX17)
-      if(COMPILER_SUPPORTS_CXX17)
-        set(${_RESULT} "-std=c++17" CACHE INTERNAL "C++17 flag")
-      else()
-        # Check for older version (before 2017)
-        CHECK_CXX_COMPILER_FLAG("-std=c++1z" COMPILER_SUPPORTS_CXX1Z)
-        if(COMPILER_SUPPORTS_CXX1Z)
-          set(${_RESULT} "-std=c++1z" CACHE INTERNAL "C++17 flag")
-        else()
-          message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++17 support. Please use a different C++ compiler.")
-        endif()
-      endif()
-    endif()
-  endif()
-endmacro()
diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt
index 39541301d..79fc67fa7 100644
--- a/examples/CMakeLists.txt
+++ b/examples/CMakeLists.txt
@@ -1,61 +1,49 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.14.0)
 project(ur_driver_examples)
 
 # find_package(ur_client_library REQUIRED)
 
-# #
-# # Check C++11 support / enable global pedantic and Wall
-# #
-include(DefineCXX17CompilerFlag)
-DEFINE_CXX_17_COMPILER_FLAG(CXX17_FLAG)
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -pedantic")
 
 add_executable(driver_example
   full_driver.cpp)
-target_compile_options(driver_example PUBLIC ${CXX17_FLAG})
 target_link_libraries(driver_example ur_client_library::urcl)
 
 add_executable(primary_pipeline_example
   primary_pipeline.cpp)
-target_compile_options(primary_pipeline_example PUBLIC ${CXX17_FLAG})
 target_link_libraries(primary_pipeline_example ur_client_library::urcl)
 
 add_executable(primary_pipeline_calibration_example
   primary_pipeline_calibration.cpp)
-target_compile_options(primary_pipeline_calibration_example PUBLIC ${CXX17_FLAG})
 target_link_libraries(primary_pipeline_calibration_example ur_client_library::urcl)
 
 add_executable(rtde_client_example
   rtde_client.cpp)
-target_compile_options(rtde_client_example PUBLIC ${CXX17_FLAG})
 target_link_libraries(rtde_client_example ur_client_library::urcl)
 
 add_executable(dashboard_example
   dashboard_example.cpp)
-target_compile_options(dashboard_example PUBLIC ${CXX17_FLAG})
 target_link_libraries(dashboard_example ur_client_library::urcl)
 
 add_executable(spline_example
 spline_example.cpp)
-target_compile_options(spline_example PUBLIC ${CXX17_FLAG})
 target_link_libraries(spline_example ur_client_library::urcl)
 
 add_executable(tool_contact_example
 tool_contact_example.cpp)
-target_compile_options(tool_contact_example PUBLIC ${CXX17_FLAG})
 target_link_libraries(tool_contact_example ur_client_library::urcl)
 
 add_executable(freedrive_example
 freedrive_example.cpp)
-target_compile_options(freedrive_example PUBLIC ${CXX17_FLAG})
 target_link_libraries(freedrive_example ur_client_library::urcl)
 
 add_executable(force_mode_example
 force_mode_example.cpp)
-target_compile_options(force_mode_example PUBLIC ${CXX17_FLAG})
 target_link_libraries(force_mode_example ur_client_library::urcl)
 
 add_executable(script_sender_example
 script_sender.cpp)
-target_compile_options(script_sender_example PUBLIC ${CXX17_FLAG})
 target_link_libraries(script_sender_example ur_client_library::urcl)
+
+add_executable(trajectory_point_interface_example
+trajectory_point_interface.cpp)
+target_link_libraries(trajectory_point_interface_example ur_client_library::urcl)
diff --git a/examples/trajectory_point_interface.cpp b/examples/trajectory_point_interface.cpp
new file mode 100644
index 000000000..5a7173b2a
--- /dev/null
+++ b/examples/trajectory_point_interface.cpp
@@ -0,0 +1,213 @@
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// Copyright 2024 Universal Robots A/S
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the {copyright_holder} nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+// -- END LICENSE BLOCK ------------------------------------------------
+
+#include <chrono>
+#include <string>
+#include <thread>
+
+#include "ur_client_library/types.h"
+#include "ur_client_library/ur/ur_driver.h"
+#include "ur_client_library/log.h"
+#include "ur_client_library/control/trajectory_point_interface.h"
+#include "ur_client_library/ur/dashboard_client.h"
+
+const std::string DEFAULT_ROBOT_IP = "192.168.56.101";
+const std::string SCRIPT_FILE = "resources/external_control.urscript";
+const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt";
+const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt";
+const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542";
+
+std::unique_ptr<urcl::DashboardClient> g_my_dashboard;
+std::unique_ptr<urcl::UrDriver> g_my_driver;
+
+std::atomic<bool> g_trajectory_done = false;
+
+// We need a callback function to register. See UrDriver's parameters for details.
+void handleRobotProgramState(bool program_running)
+{
+  // Print the text in green so we see it better
+  std::cout << "\033[1;32mProgram running: " << std::boolalpha << program_running << "\033[0m\n" << std::endl;
+}
+
+void trajDoneCallback(const urcl::control::TrajectoryResult& result)
+{
+  URCL_LOG_INFO("Trajectory done with result %d", result);
+  ;
+  g_trajectory_done = true;
+}
+
+int main(int argc, char* argv[])
+{
+  urcl::setLogLevel(urcl::LogLevel::INFO);
+  // Parse the ip arguments if given
+  std::string robot_ip = DEFAULT_ROBOT_IP;
+  if (argc > 1)
+  {
+    robot_ip = std::string(argv[1]);
+  }
+
+  // --------------- INITIALIZATION BEGIN -------------------
+  // Making the robot ready for the program by:
+  // Connect the robot Dashboard
+  g_my_dashboard.reset(new urcl::DashboardClient(robot_ip));
+  if (!g_my_dashboard->connect())
+  {
+    URCL_LOG_ERROR("Could not connect to dashboard");
+    return 1;
+  }
+
+  // // Stop program, if there is one running
+  if (!g_my_dashboard->commandStop())
+  {
+    URCL_LOG_ERROR("Could not send stop program command");
+    return 1;
+  }
+
+  // Power it off
+  if (!g_my_dashboard->commandPowerOff())
+  {
+    URCL_LOG_ERROR("Could not send Power off command");
+    return 1;
+  }
+
+  // Power it on
+  if (!g_my_dashboard->commandPowerOn())
+  {
+    URCL_LOG_ERROR("Could not send Power on command");
+    return 1;
+  }
+
+  // Release the brakes
+  if (!g_my_dashboard->commandBrakeRelease())
+  {
+    URCL_LOG_ERROR("Could not send BrakeRelease command");
+    return 1;
+  }
+
+  std::unique_ptr<urcl::ToolCommSetup> tool_comm_setup;
+  const bool headless = true;
+  g_my_driver.reset(new urcl::UrDriver(robot_ip, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState,
+                                       headless, std::move(tool_comm_setup), CALIBRATION_CHECKSUM));
+  // --------------- INITIALIZATION END -------------------
+
+  g_my_driver->registerTrajectoryDoneCallback(&trajDoneCallback);
+
+  URCL_LOG_INFO("Running MoveJ motion");
+  // --------------- MOVEJ TRAJECTORY -------------------
+  {
+    g_trajectory_done = false;
+    // Trajectory definition
+    std::vector<urcl::vector6d_t> points{ { -1.57, -1.57, 0, 0, 0, 0 }, { -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 } };
+    std::vector<double> motion_durations{ 5.0, 2.0 };
+    std::vector<double> blend_radii{ 0.1, 0.1 };
+
+    // Trajectory execution
+    g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START,
+                                               points.size() * 2);
+    for (size_t i = 0; i < points.size(); i++)
+    {
+      g_my_driver->writeTrajectoryPoint(points[i], false, motion_durations[i], blend_radii[i]);
+    }
+
+    // Same motion, but parametrized with acceleration and velocity
+    motion_durations = { 0.0, 0.0 };
+    for (size_t i = 0; i < points.size(); i++)
+    {
+      g_my_driver->writeTrajectoryPoint(points[i], 2.0, 2.0, false, motion_durations[i], blend_radii[i]);
+    }
+
+    while (!g_trajectory_done)
+    {
+      std::this_thread::sleep_for(std::chrono::milliseconds(100));
+      g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP);
+    }
+  }
+  // --------------- END MOVEJ TRAJECTORY -------------------
+
+  URCL_LOG_INFO("Running MoveL motion");
+  // --------------- MOVEL TRAJECTORY -------------------
+  {
+    g_trajectory_done = false;
+    // Trajectory definition
+    std::vector<urcl::vector6d_t> points{ { -0.203, 0.263, 0.559, 0.68, -1.083, -2.076 },
+                                          { -0.203, 0.463, 0.559, 0.68, -1.083, -2.076 } };
+    std::vector<double> motion_durations{ 5.0, 5.0 };
+    std::vector<double> blend_radii{ 0.0, 0.0 };
+
+    // Trajectory execution
+    g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START,
+                                               points.size());
+    for (size_t i = 0; i < points.size(); i++)
+    {
+      // setting the cartesian parameter makes it interpret the 6d vector as a pose and use movel
+      g_my_driver->writeTrajectoryPoint(points[i], true, motion_durations[i], blend_radii[i]);
+    }
+
+    while (!g_trajectory_done)
+    {
+      std::this_thread::sleep_for(std::chrono::milliseconds(100));
+      g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP);
+    }
+  }
+  // --------------- END MOVEL TRAJECTORY -------------------
+
+  URCL_LOG_INFO("Running a spline motion");
+  // --------------- SPLINE TRAJECTORY -------------------
+  {
+    g_trajectory_done = false;
+    // Trajectory definition
+    std::vector<urcl::vector6d_t> positions{ { -1.57, -1.57, 0, 0, 0, 0 },
+                                             { -1.57, -1.57, -1.57, 0, 0, 0 },
+                                             { -1.57, -1.57, 0, 0, 0, 0 },
+                                             { -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 } };
+    std::vector<urcl::vector6d_t> velocities{
+      { 0, 0, 0.0, 0, 0, 0 }, { 0, 0, 0.0, 0, 0, 0 }, { 0, 0, 1.5, 0, 0, 0 }, { 0, 0, 0, 0, 0, 0 }
+    };
+    std::vector<double> motion_durations{ 3.0, 3.0, 3.0, 3.0 };
+
+    // Trajectory execution
+    g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START,
+                                               positions.size());
+    for (size_t i = 0; i < positions.size(); i++)
+    {
+      g_my_driver->writeTrajectorySplinePoint(positions[i], velocities[i], motion_durations[i]);
+    }
+
+    while (!g_trajectory_done)
+    {
+      std::this_thread::sleep_for(std::chrono::milliseconds(100));
+      g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP);
+    }
+  }
+  // --------------- END MOVEJ TRAJECTORY -------------------
+
+  g_my_driver->stopControl();
+  return 0;
+}
diff --git a/include/ur_client_library/control/trajectory_point_interface.h b/include/ur_client_library/control/trajectory_point_interface.h
index e64e00350..a7768a4a0 100644
--- a/include/ur_client_library/control/trajectory_point_interface.h
+++ b/include/ur_client_library/control/trajectory_point_interface.h
@@ -29,6 +29,8 @@
 #ifndef UR_CLIENT_LIBRARY_TRAJECTORY_INTERFACE_H_INCLUDED
 #define UR_CLIENT_LIBRARY_TRAJECTORY_INTERFACE_H_INCLUDED
 
+#include <optional>
+
 #include "ur_client_library/control/reverse_interface.h"
 #include "ur_client_library/comm/control_mode.h"
 #include "ur_client_library/types.h"
@@ -44,6 +46,7 @@ namespace control
 enum class TrajectoryResult : int32_t
 {
 
+  TRAJECTORY_RESULT_UNKNOWN = -1,  ///< No result received, yet
   TRAJECTORY_RESULT_SUCCESS = 0,   ///< Successful execution
   TRAJECTORY_RESULT_CANCELED = 1,  ///< Canceled by user
   TRAJECTORY_RESULT_FAILURE = 2    ///< Aborted due to error during execution
@@ -76,6 +79,7 @@ class TrajectoryPointInterface : public ReverseInterface
 {
 public:
   static const int32_t MULT_TIME = 1000;
+  static const int MESSAGE_LENGTH = 21;
 
   TrajectoryPointInterface() = delete;
   /*!
@@ -103,6 +107,23 @@ class TrajectoryPointInterface : public ReverseInterface
   bool writeTrajectoryPoint(const vector6d_t* positions, const float goal_time, const float blend_radius,
                             const bool cartesian);
 
+  /*!
+   * \brief Writes information for a robot motion to the robot to be read by the URScript program.
+   *
+   * \param positions A vector of joint or cartesian targets for the robot
+   * \param acceleration Joint acceleration of leading axis [rad/s^2] / tool acceleration [m/s^2]
+   * for Cartesian motions
+   * \param velocity Joint speed of leading axis [rad/s] / tool speed [m/s] for Cartesian motions
+   * \param goal_time The goal time to reach the target. When a non-zero goal time is specified,
+   * this has priority over speed and acceleration settings.
+   * \param blend_radius The radius to be used for blending between control points
+   * \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(const vector6d_t* positions, const float acceleration = 1.4, const float velocity = 1.05,
+                            const float goal_time = 0, const float blend_radius = 0, const bool cartesian = false);
+
   /*!
    * \brief Writes needed information to the robot to be read by the URScript program including
    * velocity and acceleration information. Depending on the information given the robot will do quadratic (positions
@@ -131,7 +152,6 @@ 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_;
 };
 
diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h
index 358e1b58e..45d7dd2fc 100644
--- a/include/ur_client_library/ur/ur_driver.h
+++ b/include/ur_client_library/ur/ur_driver.h
@@ -272,6 +272,23 @@ class UrDriver
   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.
+   *
+   * \param positions Desired joint or cartesian positions
+   * \param cartesian True, if the point sent is cartesian, false if joint-based
+   * \param acceleration Joint acceleration of leading axis [rad/s^2] / tool acceleration [m/s^2]
+   * for Cartesian motions
+   * \param velocity Joint speed of leading axis [rad/s] / tool speed [m/s] for Cartesian motions
+   * \param goal_time Time for the robot to reach this point. When a non-zero goal time is specified,
+   * this has priority over speed and acceleration settings.
+   * \param blend_radius  The radius to be used for blending between control points
+   *
+   * \returns True on successful write.
+   */
+  bool writeTrajectoryPoint(const vector6d_t& positions, float acceleration, float velocity, const bool cartesian,
+                            const float goal_time = 0.0, const float blend_radius = 0.052);
+
   /*!
    * \brief Writes a trajectory spline point for quintic spline interpolation onto the dedicated socket.
    *
diff --git a/resources/external_control.urscript b/resources/external_control.urscript
index 05dae6c31..2bc3ab933 100644
--- a/resources/external_control.urscript
+++ b/resources/external_control.urscript
@@ -479,7 +479,9 @@ thread trajectoryThread():
       end
       # MoveJ point
       if raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_JOINT:
-        movej(q, t = tmptime, r = blend_radius)
+        acceleration = raw_point[7] / MULT_jointstate
+        velocity = raw_point[13] / MULT_jointstate
+        movej(q, a = acceleration, v = velocity, t = tmptime, r = blend_radius)
 
         # reset old acceleration
         spline_qdd = [0, 0, 0, 0, 0, 0]
@@ -487,7 +489,9 @@ thread trajectoryThread():
 
         # Movel point
       elif raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_CARTESIAN:
-        movel(p[q[0], q[1], q[2], q[3], q[4], q[5]], t = tmptime, r = blend_radius)
+        acceleration = raw_point[7] / MULT_jointstate
+        velocity = raw_point[13] / MULT_jointstate
+        movel(p[q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, t = tmptime, r = blend_radius)
 
         # reset old acceleration
         spline_qdd = [0, 0, 0, 0, 0, 0]
diff --git a/src/control/trajectory_point_interface.cpp b/src/control/trajectory_point_interface.cpp
index e88b724c0..2dcbc013c 100644
--- a/src/control/trajectory_point_interface.cpp
+++ b/src/control/trajectory_point_interface.cpp
@@ -38,7 +38,8 @@ TrajectoryPointInterface::TrajectoryPointInterface(uint32_t port) : ReverseInter
 {
 }
 
-bool TrajectoryPointInterface::writeTrajectoryPoint(const vector6d_t* positions, const float goal_time,
+bool TrajectoryPointInterface::writeTrajectoryPoint(const vector6d_t* positions, const float acceleration,
+                                                    const float velocity, const float goal_time,
                                                     const float blend_radius, const bool cartesian)
 {
   if (client_fd_ == -1)
@@ -56,16 +57,24 @@ bool TrajectoryPointInterface::writeTrajectoryPoint(const vector6d_t* positions,
       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(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(acceleration * MULT_JOINTSTATE));
+      val = htobe32(val);
+      b_pos += append(b_pos, val);
+    }
   }
   else
   {
     b_pos += 6 * sizeof(int32_t);
   }
 
-  // Fill in velocity and acceleration, not used for this point type
-  b_pos += 6 * sizeof(int32_t);
-  b_pos += 6 * sizeof(int32_t);
-
   int32_t val = static_cast<int32_t>(round(goal_time * MULT_TIME));
   val = htobe32(val);
   b_pos += append(b_pos, val);
@@ -91,6 +100,12 @@ 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 goal_time,
+                                                    const float blend_radius, const bool cartesian)
+{
+  return writeTrajectoryPoint(positions, 1.4, 1.05, goal_time, blend_radius, cartesian);
+}
+
 bool TrajectoryPointInterface::writeTrajectorySplinePoint(const vector6d_t* positions, const vector6d_t* velocities,
                                                           const vector6d_t* accelerations, const float goal_time)
 {
diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp
index 4e05a487f..88a485fdb 100644
--- a/src/ur/ur_driver.cpp
+++ b/src/ur/ur_driver.cpp
@@ -232,6 +232,13 @@ bool UrDriver::writeJointCommand(const vector6d_t& values, const comm::ControlMo
   return reverse_interface_->write(&values, control_mode, robot_receive_timeout);
 }
 
+bool UrDriver::writeTrajectoryPoint(const vector6d_t& positions, const float acceleration, const float velocity,
+                                    const bool cartesian, const float goal_time, const float blend_radius)
+{
+  return trajectory_interface_->writeTrajectoryPoint(&positions, acceleration, velocity, goal_time, blend_radius,
+                                                     cartesian);
+}
+
 bool UrDriver::writeTrajectoryPoint(const vector6d_t& positions, const bool cartesian, const float goal_time,
                                     const float blend_radius)
 {
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index db831f054..72da98804 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.14.0)
 
 set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/../CMakeModules/" ${CMAKE_MODULE_PATH})
 
@@ -21,9 +21,6 @@ if (NOT TARGET ur_client_library::urcl)
   find_package(ur_client_library REQUIRED)
 endif()
 
-# Check C++11 support
-include(DefineCXX17CompilerFlag)
-DEFINE_CXX_17_COMPILER_FLAG(CXX17_FLAG)
 
 
 # Build Tests
@@ -31,7 +28,6 @@ if (INTEGRATION_TESTS)
   # Integration tests require a robot reachable at 192.168.56.101. Therefore, they have to be
   # activated separately.
   add_executable(rtde_tests test_rtde_client.cpp)
-  target_compile_options(rtde_tests PRIVATE ${CXX17_FLAG})
   target_include_directories(rtde_tests PRIVATE ${GTEST_INCLUDE_DIRS})
   target_link_libraries(rtde_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
   gtest_add_tests(TARGET      rtde_tests
@@ -39,14 +35,12 @@ if (INTEGRATION_TESTS)
   )
 
   add_executable(dashboard_client_tests test_dashboard_client.cpp)
-  target_compile_options(dashboard_client_tests PRIVATE ${CXX17_FLAG})
   target_include_directories(dashboard_client_tests PRIVATE ${GTEST_INCLUDE_DIRS})
   target_link_libraries(dashboard_client_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
   gtest_add_tests(TARGET      dashboard_client_tests
   )
 
   add_executable(spline_tests test_spline_interpolation.cpp)
-  target_compile_options(spline_tests PRIVATE ${CXX17_FLAG})
   target_include_directories(spline_tests PRIVATE ${GTEST_INCLUDE_DIRS})
   target_link_libraries(spline_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
   gtest_add_tests(TARGET      spline_tests
@@ -54,7 +48,6 @@ if (INTEGRATION_TESTS)
   )
 
   add_executable(ur_driver_tests test_ur_driver.cpp)
-  target_compile_options(ur_driver_tests PRIVATE ${CXX17_FLAG})
   target_include_directories(ur_driver_tests PRIVATE ${GTEST_INCLUDE_DIRS})
   target_link_libraries(ur_driver_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
   gtest_add_tests(TARGET      ur_driver_tests
@@ -66,168 +59,144 @@ endif()
 
 
 add_executable(primary_parser_tests test_primary_parser.cpp)
-target_compile_options(primary_parser_tests PRIVATE ${CXX17_FLAG})
 target_include_directories(primary_parser_tests PRIVATE ${GTEST_INCLUDE_DIRS})
 target_link_libraries(primary_parser_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
 gtest_add_tests(TARGET      primary_parser_tests
 )
 
 add_executable(rtde_data_package_tests test_rtde_data_package.cpp)
-target_compile_options(rtde_data_package_tests PRIVATE ${CXX17_FLAG})
 target_include_directories(rtde_data_package_tests PRIVATE ${GTEST_INCLUDE_DIRS})
 target_link_libraries(rtde_data_package_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
 gtest_add_tests(TARGET      rtde_data_package_tests
 )
 
 add_executable(rtde_parser_tests test_rtde_parser.cpp)
-target_compile_options(rtde_parser_tests PRIVATE ${CXX17_FLAG})
 target_include_directories(rtde_parser_tests PRIVATE ${GTEST_INCLUDE_DIRS})
 target_link_libraries(rtde_parser_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
 gtest_add_tests(TARGET      rtde_parser_tests
 )
 
 add_executable(tcp_server_tests test_tcp_server.cpp)
-target_compile_options(tcp_server_tests PRIVATE ${CXX17_FLAG})
 target_include_directories(tcp_server_tests PRIVATE ${GTEST_INCLUDE_DIRS})
 target_link_libraries(tcp_server_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
 gtest_add_tests(TARGET      tcp_server_tests
 )
 
 add_executable(script_sender_tests test_script_sender.cpp)
-target_compile_options(script_sender_tests PRIVATE ${CXX17_FLAG})
 target_include_directories(script_sender_tests PRIVATE ${GTEST_INCLUDE_DIRS})
 target_link_libraries(script_sender_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
 gtest_add_tests(TARGET      script_sender_tests
 )
 
 add_executable(reverse_interface_tests test_reverse_interface.cpp)
-target_compile_options(reverse_interface_tests PRIVATE ${CXX17_FLAG})
 target_include_directories(reverse_interface_tests PRIVATE ${GTEST_INCLUDE_DIRS})
 target_link_libraries(reverse_interface_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
 gtest_add_tests(TARGET reverse_interface_tests
 )
 
 add_executable(trajectory_point_interface_tests test_trajectory_point_interface.cpp)
-target_compile_options(trajectory_point_interface_tests PRIVATE ${CXX17_FLAG})
 target_include_directories(trajectory_point_interface_tests PRIVATE ${GTEST_INCLUDE_DIRS})
 target_link_libraries(trajectory_point_interface_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
 gtest_add_tests(TARGET trajectory_point_interface_tests
 )
 
 add_executable(rtde_control_package_pause_tests test_rtde_control_package_pause.cpp)
-target_compile_options(rtde_control_package_pause_tests PRIVATE ${CXX17_FLAG})
 target_include_directories(rtde_control_package_pause_tests PRIVATE ${GTEST_INCLUDE_DIRS})
 target_link_libraries(rtde_control_package_pause_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
 gtest_add_tests(TARGET rtde_control_package_pause_tests
 )
 
 add_executable(rtde_control_package_start_tests test_rtde_control_package_start.cpp)
-target_compile_options(rtde_control_package_start_tests PRIVATE ${CXX17_FLAG})
 target_include_directories(rtde_control_package_start_tests PRIVATE ${GTEST_INCLUDE_DIRS})
 target_link_libraries(rtde_control_package_start_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
 gtest_add_tests(TARGET rtde_control_package_start_tests
 )
 
 add_executable(rtde_control_package_setup_outputs_tests test_rtde_control_package_setup_outputs.cpp)
-target_compile_options(rtde_control_package_setup_outputs_tests PRIVATE ${CXX17_FLAG})
 target_include_directories(rtde_control_package_setup_outputs_tests PRIVATE ${GTEST_INCLUDE_DIRS})
 target_link_libraries(rtde_control_package_setup_outputs_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
 gtest_add_tests(TARGET rtde_control_package_setup_outputs_tests
 )
 
 add_executable(rtde_control_package_setup_inputs_tests test_rtde_control_package_setup_inputs.cpp)
-target_compile_options(rtde_control_package_setup_inputs_tests PRIVATE ${CXX17_FLAG})
 target_include_directories(rtde_control_package_setup_inputs_tests PRIVATE ${GTEST_INCLUDE_DIRS})
 target_link_libraries(rtde_control_package_setup_inputs_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
 gtest_add_tests(TARGET rtde_control_package_setup_inputs_tests
 )
 
 add_executable(rtde_get_urcontrol_version_tests test_rtde_get_urcontrol_version.cpp)
-target_compile_options(rtde_get_urcontrol_version_tests PRIVATE ${CXX17_FLAG})
 target_include_directories(rtde_get_urcontrol_version_tests PRIVATE ${GTEST_INCLUDE_DIRS})
 target_link_libraries(rtde_get_urcontrol_version_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
 gtest_add_tests(TARGET rtde_get_urcontrol_version_tests
 )
 
 add_executable(rtde_request_protocol_version_tests test_rtde_request_protocol_version.cpp)
-target_compile_options(rtde_request_protocol_version_tests PRIVATE ${CXX17_FLAG})
 target_include_directories(rtde_request_protocol_version_tests PRIVATE ${GTEST_INCLUDE_DIRS})
 target_link_libraries(rtde_request_protocol_version_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
 gtest_add_tests(TARGET rtde_request_protocol_version_tests
 )
 
 add_executable(rtde_writer_tests test_rtde_writer.cpp)
-target_compile_options(rtde_writer_tests PRIVATE ${CXX17_FLAG})
 target_include_directories(rtde_writer_tests PRIVATE ${GTEST_INCLUDE_DIRS})
 target_link_libraries(rtde_writer_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
 gtest_add_tests(TARGET rtde_writer_tests
 )
 
 add_executable(version_information_tests test_version_information.cpp)
-target_compile_options(version_information_tests PRIVATE ${CXX17_FLAG})
 target_include_directories(version_information_tests PRIVATE ${GTEST_INCLUDE_DIRS})
 target_link_libraries(version_information_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
 gtest_add_tests(TARGET version_information_tests
 )
 
 add_executable(bin_parser_tests test_bin_parser.cpp)
-target_compile_options(bin_parser_tests PRIVATE ${CXX17_FLAG})
 target_include_directories(bin_parser_tests PRIVATE ${GTEST_INCLUDE_DIRS})
 target_link_libraries(bin_parser_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
 gtest_add_tests(TARGET bin_parser_tests
 )
 
 add_executable(package_serializer_tests test_package_serializer.cpp)
-target_compile_options(package_serializer_tests PRIVATE ${CXX17_FLAG})
 target_include_directories(package_serializer_tests PRIVATE ${GTEST_INCLUDE_DIRS})
 target_link_libraries(package_serializer_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
 gtest_add_tests(TARGET package_serializer_tests
 )
 
 add_executable(tcp_socket_tests test_tcp_socket.cpp)
-target_compile_options(tcp_socket_tests PRIVATE ${CXX17_FLAG})
 target_include_directories(tcp_socket_tests PRIVATE ${GTEST_INCLUDE_DIRS})
 target_link_libraries(tcp_socket_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
 gtest_add_tests(TARGET tcp_socket_tests
 )
 
 add_executable(stream_tests test_stream.cpp)
-target_compile_options(stream_tests PRIVATE ${CXX17_FLAG})
 target_include_directories(stream_tests PRIVATE ${GTEST_INCLUDE_DIRS})
 target_link_libraries(stream_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
 gtest_add_tests(TARGET stream_tests
 )
 
 add_executable(producer_tests test_producer.cpp)
-target_compile_options(producer_tests PRIVATE ${CXX17_FLAG})
 target_include_directories(producer_tests PRIVATE ${GTEST_INCLUDE_DIRS})
 target_link_libraries(producer_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
 gtest_add_tests(TARGET producer_tests
 )
 
 add_executable(pipeline_tests test_pipeline.cpp)
-target_compile_options(pipeline_tests PRIVATE ${CXX17_FLAG})
 target_include_directories(pipeline_tests PRIVATE ${GTEST_INCLUDE_DIRS})
 target_link_libraries(pipeline_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
 gtest_add_tests(TARGET pipeline_tests
 )
 
 add_executable(script_command_interface_tests test_script_command_interface.cpp)
-target_compile_options(script_command_interface_tests PRIVATE ${CXX17_FLAG})
 target_include_directories(script_command_interface_tests PRIVATE ${GTEST_INCLUDE_DIRS})
 target_link_libraries(script_command_interface_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
 gtest_add_tests(TARGET script_command_interface_tests
 )
 
 add_executable(robot_receive_timeout_tests test_robot_receive_timeout.cpp)
-target_compile_options(robot_receive_timeout_tests PRIVATE ${CXX17_FLAG})
 target_include_directories(robot_receive_timeout_tests PRIVATE ${GTEST_INCLUDE_DIRS})
 target_link_libraries(robot_receive_timeout_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
 gtest_add_tests(TARGET robot_receive_timeout_tests
 )
 
 add_executable(control_mode_tests test_control_mode.cpp)
-target_compile_options(control_mode_tests PRIVATE ${CXX17_FLAG})
 target_include_directories(control_mode_tests PRIVATE ${GTEST_INCLUDE_DIRS})
 target_link_libraries(control_mode_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
 gtest_add_tests(TARGET control_mode_tests
diff --git a/tests/test_trajectory_point_interface.cpp b/tests/test_trajectory_point_interface.cpp
index c335b3e4e..c834e470b 100644
--- a/tests/test_trajectory_point_interface.cpp
+++ b/tests/test_trajectory_point_interface.cpp
@@ -63,16 +63,15 @@ class TrajectoryPointInterfaceTest : public ::testing::Test
                      int32_t& blend_radius_or_spline_type, int32_t& motion_type)
     {
       // Read message
-      uint8_t buf[sizeof(int32_t) * 21];
+      uint8_t buf[sizeof(int32_t) * urcl::control::TrajectoryPointInterface::MESSAGE_LENGTH];
       uint8_t* b_pos = buf;
       size_t read = 0;
-      size_t remainder = sizeof(int32_t) * 21;
+      size_t remainder = sizeof(int32_t) * urcl::control::TrajectoryPointInterface::MESSAGE_LENGTH;
       while (remainder > 0)
       {
         if (!TCPSocket::read(b_pos, remainder, read))
         {
-          std::cout << "Failed to read from socket, this should not happen during a test!" << std::endl;
-          break;
+          throw(std::runtime_error("Failed to read from socket, this should not happen during a test!"));
         }
         b_pos += read;
         remainder -= read;
@@ -135,6 +134,14 @@ class TrajectoryPointInterfaceTest : public ::testing::Test
       return vel;
     }
 
+    vector6int32_t getAcceleration()
+    {
+      int32_t goal_time, blend_radius_or_spline_type, motion_type;
+      vector6int32_t pos, vel, acc;
+      readMessage(pos, vel, acc, goal_time, blend_radius_or_spline_type, motion_type);
+      return acc;
+    }
+
     int32_t getGoalTime()
     {
       int32_t goal_time, blend_radius_or_spline_type, motion_type;
@@ -218,13 +225,13 @@ class TrajectoryPointInterfaceTest : public ::testing::Test
 private:
   std::condition_variable trajectory_end_;
   std::mutex trajectory_end_mutex_;
-  control::TrajectoryResult result_;
+  control::TrajectoryResult result_ = control::TrajectoryResult::TRAJECTORY_RESULT_UNKNOWN;
 };
 
 TEST_F(TrajectoryPointInterfaceTest, write_postions)
 {
   urcl::vector6d_t send_positions = { 1.2, 3.1, 2.2, -3.4, -1.1, -1.2 };
-  traj_point_interface_->writeTrajectoryPoint(&send_positions, 0, 0, 0);
+  traj_point_interface_->writeTrajectoryPoint(&send_positions, 0, 0, false);
   vector6int32_t received_positions = client_->getPosition();
 
   EXPECT_EQ(send_positions[0], ((double)received_positions[0]) / traj_point_interface_->MULT_JOINTSTATE);
@@ -361,9 +368,30 @@ TEST_F(TrajectoryPointInterfaceTest, write_goal_time)
 {
   urcl::vector6d_t send_positions = { 0, 0, 0, 0, 0, 0 };
   float send_goal_time = 0.5;
-  traj_point_interface_->writeTrajectoryPoint(&send_positions, send_goal_time, 0, 0);
+  traj_point_interface_->writeTrajectoryPoint(&send_positions, send_goal_time, 0, false);
+  int32_t received_goal_time = client_->getGoalTime();
+
+  EXPECT_EQ(send_goal_time, ((float)received_goal_time) / traj_point_interface_->MULT_TIME);
+}
+
+TEST_F(TrajectoryPointInterfaceTest, write_acceleration_velocity)
+{
+  urcl::vector6d_t send_positions = { 0, 0, 0, 0, 0, 0 };
+  float send_move_acceleration = 0.123;
+  float send_move_velocity = 0.456;
+  float send_goal_time = 0.5;
+  traj_point_interface_->writeTrajectoryPoint(&send_positions, send_move_acceleration, send_move_velocity,
+                                              send_goal_time, 0, 0);
+  int32_t received_move_acceleration = client_->getAcceleration()[0];
+  traj_point_interface_->writeTrajectoryPoint(&send_positions, send_move_acceleration, send_move_velocity,
+                                              send_goal_time, 0, 0);
+  int32_t received_move_velocity = client_->getVelocity()[0];
+  traj_point_interface_->writeTrajectoryPoint(&send_positions, send_move_acceleration, send_move_velocity,
+                                              send_goal_time, 0, 0);
   int32_t received_goal_time = client_->getGoalTime();
 
+  EXPECT_EQ(send_move_acceleration, ((float)received_move_acceleration) / traj_point_interface_->MULT_JOINTSTATE);
+  EXPECT_EQ(send_move_velocity, ((float)received_move_velocity) / traj_point_interface_->MULT_JOINTSTATE);
   EXPECT_EQ(send_goal_time, ((float)received_goal_time) / traj_point_interface_->MULT_TIME);
 }
 
@@ -371,7 +399,7 @@ TEST_F(TrajectoryPointInterfaceTest, write_blend_radius)
 {
   urcl::vector6d_t send_positions = { 0, 0, 0, 0, 0, 0 };
   float send_blend_radius = 0.5;
-  traj_point_interface_->writeTrajectoryPoint(&send_positions, 0, send_blend_radius, 0);
+  traj_point_interface_->writeTrajectoryPoint(&send_positions, 0, send_blend_radius, false);
   int32_t received_blend_radius = client_->getBlendRadius();
 
   EXPECT_EQ(send_blend_radius, ((float)received_blend_radius) / traj_point_interface_->MULT_TIME);