Skip to content

Commit c88ce84

Browse files
urrskurrsk
committed
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
1 parent a61a823 commit c88ce84

File tree

3 files changed

+16
-13
lines changed

3 files changed

+16
-13
lines changed

Diff for: ‎src/control/reverse_interface.cpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,7 @@
2727
//----------------------------------------------------------------------
2828

2929
#include <ur_client_library/control/reverse_interface.h>
30+
#include <math.h>
3031

3132
namespace urcl
3233
{
@@ -62,7 +63,7 @@ bool ReverseInterface::write(const vector6d_t* positions, const comm::ControlMod
6263
{
6364
for (auto const& pos : *positions)
6465
{
65-
int32_t val = static_cast<int32_t>(pos * MULT_JOINTSTATE);
66+
int32_t val = static_cast<int32_t>(round(pos * MULT_JOINTSTATE));
6667
val = htobe32(val);
6768
b_pos += append(b_pos, val);
6869
}

Diff for: ‎src/control/script_command_interface.cpp

+6-5
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,7 @@
2727
//----------------------------------------------------------------------
2828

2929
#include <ur_client_library/control/script_command_interface.h>
30+
#include <math.h>
3031

3132
namespace urcl
3233
{
@@ -64,12 +65,12 @@ bool ScriptCommandInterface::setPayload(const double mass, const vector3d_t* cog
6465
int32_t val = htobe32(toUnderlying(ScriptCommand::SET_PAYLOAD));
6566
b_pos += append(b_pos, val);
6667

67-
val = htobe32(static_cast<int32_t>(mass * MULT_JOINTSTATE));
68+
val = htobe32(static_cast<int32_t>(round(mass * MULT_JOINTSTATE)));
6869
b_pos += append(b_pos, val);
6970

7071
for (auto const& center_of_mass : *cog)
7172
{
72-
val = htobe32(static_cast<int32_t>(center_of_mass * MULT_JOINTSTATE));
73+
val = htobe32(static_cast<int32_t>(round(center_of_mass * MULT_JOINTSTATE)));
7374
b_pos += append(b_pos, val);
7475
}
7576

@@ -118,7 +119,7 @@ bool ScriptCommandInterface::startForceMode(const vector6d_t* task_frame, const
118119

119120
for (auto const& frame : *task_frame)
120121
{
121-
val = htobe32(static_cast<int32_t>(frame * MULT_JOINTSTATE));
122+
val = htobe32(static_cast<int32_t>(round(frame * MULT_JOINTSTATE)));
122123
b_pos += append(b_pos, val);
123124
}
124125

@@ -130,7 +131,7 @@ bool ScriptCommandInterface::startForceMode(const vector6d_t* task_frame, const
130131

131132
for (auto const& force_torque : *wrench)
132133
{
133-
val = htobe32(static_cast<int32_t>(force_torque * MULT_JOINTSTATE));
134+
val = htobe32(static_cast<int32_t>(round(force_torque * MULT_JOINTSTATE)));
134135
b_pos += append(b_pos, val);
135136
}
136137

@@ -139,7 +140,7 @@ bool ScriptCommandInterface::startForceMode(const vector6d_t* task_frame, const
139140

140141
for (auto const& lim : *limits)
141142
{
142-
val = htobe32(static_cast<int32_t>(lim * MULT_JOINTSTATE));
143+
val = htobe32(static_cast<int32_t>(round(lim * MULT_JOINTSTATE)));
143144
b_pos += append(b_pos, val);
144145
}
145146

Diff for: ‎src/control/trajectory_point_interface.cpp

+8-7
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,7 @@
2828

2929
#include <ur_client_library/control/trajectory_point_interface.h>
3030
#include <ur_client_library/exceptions.h>
31+
#include <math.h>
3132

3233
namespace urcl
3334
{
@@ -51,7 +52,7 @@ bool TrajectoryPointInterface::writeTrajectoryPoint(const vector6d_t* positions,
5152
{
5253
for (auto const& pos : *positions)
5354
{
54-
int32_t val = static_cast<int32_t>(pos * MULT_JOINTSTATE);
55+
int32_t val = static_cast<int32_t>(round(pos * MULT_JOINTSTATE));
5556
val = htobe32(val);
5657
b_pos += append(b_pos, val);
5758
}
@@ -65,11 +66,11 @@ bool TrajectoryPointInterface::writeTrajectoryPoint(const vector6d_t* positions,
6566
b_pos += 6 * sizeof(int32_t);
6667
b_pos += 6 * sizeof(int32_t);
6768

68-
int32_t val = static_cast<int32_t>(goal_time * MULT_TIME);
69+
int32_t val = static_cast<int32_t>(round(goal_time * MULT_TIME));
6970
val = htobe32(val);
7071
b_pos += append(b_pos, val);
7172

72-
val = static_cast<int32_t>(blend_radius * MULT_TIME);
73+
val = static_cast<int32_t>(round(blend_radius * MULT_TIME));
7374
val = htobe32(val);
7475
b_pos += append(b_pos, val);
7576

@@ -107,7 +108,7 @@ bool TrajectoryPointInterface::writeTrajectorySplinePoint(const vector6d_t* posi
107108
{
108109
for (auto const& pos : *positions)
109110
{
110-
int32_t val = static_cast<int32_t>(pos * MULT_JOINTSTATE);
111+
int32_t val = static_cast<int32_t>(round(pos * MULT_JOINTSTATE));
111112
val = htobe32(val);
112113
b_pos += append(b_pos, val);
113114
}
@@ -123,7 +124,7 @@ bool TrajectoryPointInterface::writeTrajectorySplinePoint(const vector6d_t* posi
123124
spline_type = control::TrajectorySplineType::SPLINE_CUBIC;
124125
for (auto const& vel : *velocities)
125126
{
126-
int32_t val = static_cast<int32_t>(vel * MULT_JOINTSTATE);
127+
int32_t val = static_cast<int32_t>(round(vel * MULT_JOINTSTATE));
127128
val = htobe32(val);
128129
b_pos += append(b_pos, val);
129130
}
@@ -139,7 +140,7 @@ bool TrajectoryPointInterface::writeTrajectorySplinePoint(const vector6d_t* posi
139140
spline_type = control::TrajectorySplineType::SPLINE_QUINTIC;
140141
for (auto const& acc : *accelerations)
141142
{
142-
int32_t val = static_cast<int32_t>(acc * MULT_JOINTSTATE);
143+
int32_t val = static_cast<int32_t>(round(acc * MULT_JOINTSTATE));
143144
val = htobe32(val);
144145
b_pos += append(b_pos, val);
145146
}
@@ -149,7 +150,7 @@ bool TrajectoryPointInterface::writeTrajectorySplinePoint(const vector6d_t* posi
149150
b_pos += 6 * sizeof(int32_t);
150151
}
151152

152-
int32_t val = static_cast<int32_t>(goal_time * MULT_TIME);
153+
int32_t val = static_cast<int32_t>(round(goal_time * MULT_TIME));
153154
val = htobe32(val);
154155
b_pos += append(b_pos, val);
155156

0 commit comments

Comments
 (0)