diff --git a/src/control/reverse_interface.cpp b/src/control/reverse_interface.cpp
index 8f38cb0d..1e4465bd 100644
--- a/src/control/reverse_interface.cpp
+++ b/src/control/reverse_interface.cpp
@@ -27,6 +27,7 @@
 //----------------------------------------------------------------------
 
 #include <ur_client_library/control/reverse_interface.h>
+#include <math.h>
 
 namespace urcl
 {
@@ -62,7 +63,7 @@ bool ReverseInterface::write(const vector6d_t* positions, const comm::ControlMod
   {
     for (auto const& pos : *positions)
     {
-      int32_t val = static_cast<int32_t>(pos * MULT_JOINTSTATE);
+      int32_t val = static_cast<int32_t>(round(pos * MULT_JOINTSTATE));
       val = htobe32(val);
       b_pos += append(b_pos, val);
     }
diff --git a/src/control/script_command_interface.cpp b/src/control/script_command_interface.cpp
index 9ceef62d..f544f49a 100644
--- a/src/control/script_command_interface.cpp
+++ b/src/control/script_command_interface.cpp
@@ -27,6 +27,7 @@
 //----------------------------------------------------------------------
 
 #include <ur_client_library/control/script_command_interface.h>
+#include <math.h>
 
 namespace urcl
 {
@@ -64,12 +65,12 @@ bool ScriptCommandInterface::setPayload(const double mass, const vector3d_t* cog
   int32_t val = htobe32(toUnderlying(ScriptCommand::SET_PAYLOAD));
   b_pos += append(b_pos, val);
 
-  val = htobe32(static_cast<int32_t>(mass * MULT_JOINTSTATE));
+  val = htobe32(static_cast<int32_t>(round(mass * MULT_JOINTSTATE)));
   b_pos += append(b_pos, val);
 
   for (auto const& center_of_mass : *cog)
   {
-    val = htobe32(static_cast<int32_t>(center_of_mass * MULT_JOINTSTATE));
+    val = htobe32(static_cast<int32_t>(round(center_of_mass * MULT_JOINTSTATE)));
     b_pos += append(b_pos, val);
   }
 
@@ -92,7 +93,7 @@ bool ScriptCommandInterface::setToolVoltage(const ToolVoltage voltage)
   int32_t val = htobe32(toUnderlying(ScriptCommand::SET_TOOL_VOLTAGE));
   b_pos += append(b_pos, val);
 
-  val = htobe32(toUnderlying(voltage) * MULT_JOINTSTATE);
+  val = htobe32(round(toUnderlying(voltage) * MULT_JOINTSTATE));
   b_pos += append(b_pos, val);
 
   // writing zeros to allow usage with other script commands
@@ -118,28 +119,28 @@ bool ScriptCommandInterface::startForceMode(const vector6d_t* task_frame, const
 
   for (auto const& frame : *task_frame)
   {
-    val = htobe32(static_cast<int32_t>(frame * MULT_JOINTSTATE));
+    val = htobe32(static_cast<int32_t>(round(frame * MULT_JOINTSTATE)));
     b_pos += append(b_pos, val);
   }
 
   for (auto const& selection : *selection_vector)
   {
-    val = htobe32(static_cast<int32_t>(selection * MULT_JOINTSTATE));
+    val = htobe32(static_cast<int32_t>(round(selection * MULT_JOINTSTATE)));
     b_pos += append(b_pos, val);
   }
 
   for (auto const& force_torque : *wrench)
   {
-    val = htobe32(static_cast<int32_t>(force_torque * MULT_JOINTSTATE));
+    val = htobe32(static_cast<int32_t>(round(force_torque * MULT_JOINTSTATE)));
     b_pos += append(b_pos, val);
   }
 
-  val = htobe32(static_cast<int32_t>(type * MULT_JOINTSTATE));
+  val = htobe32(static_cast<int32_t>(round(type * MULT_JOINTSTATE)));
   b_pos += append(b_pos, val);
 
   for (auto const& lim : *limits)
   {
-    val = htobe32(static_cast<int32_t>(lim * MULT_JOINTSTATE));
+    val = htobe32(static_cast<int32_t>(round(lim * MULT_JOINTSTATE)));
     b_pos += append(b_pos, val);
   }
 
diff --git a/src/control/trajectory_point_interface.cpp b/src/control/trajectory_point_interface.cpp
index 4581678a..e88b724c 100644
--- a/src/control/trajectory_point_interface.cpp
+++ b/src/control/trajectory_point_interface.cpp
@@ -28,6 +28,7 @@
 
 #include <ur_client_library/control/trajectory_point_interface.h>
 #include <ur_client_library/exceptions.h>
+#include <math.h>
 
 namespace urcl
 {
@@ -51,7 +52,7 @@ bool TrajectoryPointInterface::writeTrajectoryPoint(const vector6d_t* positions,
   {
     for (auto const& pos : *positions)
     {
-      int32_t val = static_cast<int32_t>(pos * MULT_JOINTSTATE);
+      int32_t val = static_cast<int32_t>(round(pos * MULT_JOINTSTATE));
       val = htobe32(val);
       b_pos += append(b_pos, val);
     }
@@ -65,11 +66,11 @@ bool TrajectoryPointInterface::writeTrajectoryPoint(const vector6d_t* positions,
   b_pos += 6 * sizeof(int32_t);
   b_pos += 6 * sizeof(int32_t);
 
-  int32_t val = static_cast<int32_t>(goal_time * MULT_TIME);
+  int32_t val = static_cast<int32_t>(round(goal_time * MULT_TIME));
   val = htobe32(val);
   b_pos += append(b_pos, val);
 
-  val = static_cast<int32_t>(blend_radius * MULT_TIME);
+  val = static_cast<int32_t>(round(blend_radius * MULT_TIME));
   val = htobe32(val);
   b_pos += append(b_pos, val);
 
@@ -107,7 +108,7 @@ bool TrajectoryPointInterface::writeTrajectorySplinePoint(const vector6d_t* posi
   {
     for (auto const& pos : *positions)
     {
-      int32_t val = static_cast<int32_t>(pos * MULT_JOINTSTATE);
+      int32_t val = static_cast<int32_t>(round(pos * MULT_JOINTSTATE));
       val = htobe32(val);
       b_pos += append(b_pos, val);
     }
@@ -123,7 +124,7 @@ bool TrajectoryPointInterface::writeTrajectorySplinePoint(const vector6d_t* posi
     spline_type = control::TrajectorySplineType::SPLINE_CUBIC;
     for (auto const& vel : *velocities)
     {
-      int32_t val = static_cast<int32_t>(vel * MULT_JOINTSTATE);
+      int32_t val = static_cast<int32_t>(round(vel * MULT_JOINTSTATE));
       val = htobe32(val);
       b_pos += append(b_pos, val);
     }
@@ -139,7 +140,7 @@ bool TrajectoryPointInterface::writeTrajectorySplinePoint(const vector6d_t* posi
     spline_type = control::TrajectorySplineType::SPLINE_QUINTIC;
     for (auto const& acc : *accelerations)
     {
-      int32_t val = static_cast<int32_t>(acc * MULT_JOINTSTATE);
+      int32_t val = static_cast<int32_t>(round(acc * MULT_JOINTSTATE));
       val = htobe32(val);
       b_pos += append(b_pos, val);
     }
@@ -149,7 +150,7 @@ bool TrajectoryPointInterface::writeTrajectorySplinePoint(const vector6d_t* posi
     b_pos += 6 * sizeof(int32_t);
   }
 
-  int32_t val = static_cast<int32_t>(goal_time * MULT_TIME);
+  int32_t val = static_cast<int32_t>(round(goal_time * MULT_TIME));
   val = htobe32(val);
   b_pos += append(b_pos, val);