Add separate rounding in the conversion from float to int32

Identified that static cast can convert wrong when the float value is around but not zero (1e-322) resulting in a large number
UniversalRobots · Aug 7, 2023 · c88ce84 · c88ce84
1 parent a61a823
commit c88ce84
Show file tree

Hide file tree

Showing 3 changed files with 16 additions and 13 deletions.
diff --git 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
@@ -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);
   }
 
@@ -118,7 +119,7 @@ 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);
   }
 
@@ -130,7 +131,7 @@ bool ScriptCommandInterface::startForceMode(const vector6d_t* task_frame, const
 
   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);
   }
 
@@ -139,7 +140,7 @@ bool ScriptCommandInterface::startForceMode(const vector6d_t* task_frame, const
 
   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
@@ -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);