Skip to content

Commit

Permalink
Add separate rounding in the conversion from float to int32
Browse files Browse the repository at this point in the history
Identified that static cast can convert wrong when the float value is
around but not zero (1e-322) resulting in a large number
  • Loading branch information
urrsk committed Aug 7, 2023
1 parent a61a823 commit c88ce84
Show file tree
Hide file tree
Showing 3 changed files with 16 additions and 13 deletions.
3 changes: 2 additions & 1 deletion src/control/reverse_interface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@
//----------------------------------------------------------------------

#include <ur_client_library/control/reverse_interface.h>
#include <math.h>

namespace urcl
{
Expand Down Expand Up @@ -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);
}
Expand Down
11 changes: 6 additions & 5 deletions src/control/script_command_interface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@
//----------------------------------------------------------------------

#include <ur_client_library/control/script_command_interface.h>
#include <math.h>

namespace urcl
{
Expand Down Expand Up @@ -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);
}

Expand Down Expand Up @@ -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);
}

Expand All @@ -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);
}

Expand All @@ -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);
}

Expand Down
15 changes: 8 additions & 7 deletions src/control/trajectory_point_interface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@

#include <ur_client_library/control/trajectory_point_interface.h>
#include <ur_client_library/exceptions.h>
#include <math.h>

namespace urcl
{
Expand All @@ -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);
}
Expand All @@ -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);

Expand Down Expand Up @@ -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);
}
Expand All @@ -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);
}
Expand All @@ -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);
}
Expand All @@ -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);

Expand Down

0 comments on commit c88ce84

Please sign in to comment.