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
urrsk committed Aug 4, 2023
1 parent ff93f6c commit 6c451b9
Show file tree
Hide file tree
Showing 3 changed files with 19 additions and 16 deletions.
3 changes: 2 additions & 1 deletion src/control/reverse_interface.cpp
View file
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
17 changes: 9 additions & 8 deletions src/control/script_command_interface.cpp
View file
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 All @@ -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
Expand All @@ -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);
}

Expand Down
15 changes: 8 additions & 7 deletions src/control/trajectory_point_interface.cpp
View file
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 6c451b9

Please sign in to comment.