Skip to content

Fix wrong conversion signed_to_unsigned and viceversa in dynamixel_client #22

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
PasMarra wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Fix wrong conversion signed_to_unsigned and viceversa in dynamixel_client #22

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions cpp/include/leap_hand_utils/dynamixel_client.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,8 +37,8 @@

class DynamixelClient;
void dynamixel_cleanup_handler(std::set<DynamixelClient> open_clients);
int signed_to_unsigned(int value, int size);
int unsigned_to_signed(int value, int size);
uint64_t signed_to_unsigned(int64_t value, int size_bytes);
int64_t unsigned_to_signed(uint64_t value, int size_bytes);


class DynamixelPosVelCurReader;
Expand Down
88 changes: 52 additions & 36 deletions cpp/src/dynamixel_client.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,8 @@
#include <chrono>
#include <thread>

#include <cstdint>

#include <leap_hand_utils/dynamixel_client.h>


Expand All @@ -21,23 +23,37 @@ void dynamixel_cleanup_handler(std::set<DynamixelClient*> open_clients) {
}
}

int signed_to_unsigned(int value, int size) {
/* Converts the given value to its unsigned representation */
uint64_t signed_to_unsigned(int64_t value, int size_bytes) {
uint64_t bit_size = 8ULL * static_cast<uint64_t>(size_bytes);
// avoid UB for shifts; bit_size <= 64 expected
if (bit_size >= 64) {
uint64_t mod = 0; // 2^64 would overflow uint64_t (wraps to 0) — callers shouldn't request >8 bytes
return static_cast<uint64_t>(value);
}
uint64_t mod = 1ULL << bit_size;
if (value < 0) {
int bit_size = 8 * size;
int max_value = (1 << bit_size) - 1;
value += max_value;
return mod + static_cast<int64_t>(value);
}
return value;
return static_cast<uint64_t>(value);
}

int unsigned_to_signed(int value, int size) {
/* Converts the given value from its unsigned representation */
int bit_size = 8 * size;
if ((value & (1 << (bit_size - 1))) != 0) {
value = -((1 << bit_size) - value);
int64_t unsigned_to_signed(uint64_t value, int size_bytes) {
uint64_t bit_size = 8ULL * static_cast<uint64_t>(size_bytes);
if (bit_size >= 64) {
// for 64 bits signed interpretation, if msb set, subtract 2^64
uint64_t sign_mask = 1ULL << 63;
uint64_t mod = 0; // 2^64 wraps to 0; treat specially
if (value & sign_mask) {
return static_cast<int64_t>(value); // best-effort; large negative cannot be represented cleanly here
}
return static_cast<int64_t>(value);
}
return value;
uint64_t sign_mask = 1ULL << (bit_size - 1);
uint64_t mod = 1ULL << bit_size;
if (value & sign_mask) {
return static_cast<int64_t>(value - mod);
}
return static_cast<int64_t>(value);
}


Expand Down Expand Up @@ -547,15 +563,15 @@ void DynamixelPosVelCurReader::_initialize_data() {

void DynamixelPosVelCurReader::_update_data(int index, int motor_id) {
/* Updates the data index for the given motor ID */
uint32_t cur {operation.getData(motor_id, ADDR_PRESENT_CURRENT, LEN_PRESENT_CURRENT)};
uint32_t vel {operation.getData(motor_id, ADDR_PRESENT_VELOCITY, LEN_PRESENT_VELOCITY)};
uint32_t pos {operation.getData(motor_id, ADDR_PRESENT_POSITION, LEN_PRESENT_POSITION)};
cur = unsigned_to_signed(cur, 2);
vel = unsigned_to_signed(vel, 4);
pos = unsigned_to_signed(pos, 4);
_pos_data(index) = float(pos) * pos_scale;
_vel_data(index) = float(vel) * vel_scale;
_cur_data(index) = float(cur) * cur_scale;
uint64_t raw_cur = operation.getData(motor_id, ADDR_PRESENT_CURRENT, LEN_PRESENT_CURRENT);
uint64_t raw_vel = operation.getData(motor_id, ADDR_PRESENT_VELOCITY, LEN_PRESENT_VELOCITY);
uint64_t raw_pos = operation.getData(motor_id, ADDR_PRESENT_POSITION, LEN_PRESENT_POSITION);
int64_t cur = unsigned_to_signed(raw_cur, 2);
int64_t vel = unsigned_to_signed(raw_vel, 4);
int64_t pos = unsigned_to_signed(raw_pos, 4);
_pos_data(index) = static_cast<float>(pos) * pos_scale;
_vel_data(index) = static_cast<float>(vel) * vel_scale;
_cur_data(index) = static_cast<float>(cur) * cur_scale;
}

std::vector<Eigen::MatrixXd> DynamixelPosVelCurReader::_get_data() {
Expand Down Expand Up @@ -631,12 +647,12 @@ void DynamixelPosVelReader::_initialize_data() {

void DynamixelPosVelReader::_update_data(int index, int motor_id) {
/* Updates the data index for the given motor ID */
uint32_t vel {operation.getData(motor_id, ADDR_PRESENT_VELOCITY, LEN_PRESENT_VELOCITY)};
uint32_t pos {operation.getData(motor_id, ADDR_PRESENT_POSITION, LEN_PRESENT_POSITION)};
vel = unsigned_to_signed(vel, 4);
pos = unsigned_to_signed(pos, 4);
_pos_data(index) = float(pos) * pos_scale;
_vel_data(index) = float(vel) * vel_scale;
uint64_t raw_vel = operation.getData(motor_id, ADDR_PRESENT_VELOCITY, LEN_PRESENT_VELOCITY);
uint64_t raw_pos = operation.getData(motor_id, ADDR_PRESENT_POSITION, LEN_PRESENT_POSITION);
int64_t vel = unsigned_to_signed(raw_vel, 4);
int64_t pos = unsigned_to_signed(raw_pos, 4);
_pos_data(index) = static_cast<float>(pos) * pos_scale;
_vel_data(index) = static_cast<float>(vel) * vel_scale;
}

std::vector<Eigen::MatrixXd> DynamixelPosVelReader::_get_data() {
Expand Down Expand Up @@ -711,9 +727,9 @@ void DynamixelPosReader::_initialize_data() {

void DynamixelPosReader::_update_data(int index, int motor_id) {
/* Updates the data index for the given motor ID */
uint32_t pos {operation.getData(motor_id, ADDR_PRESENT_POSITION, LEN_PRESENT_POSITION)};
pos = unsigned_to_signed(pos, 4);
_pos_data(index) = float(pos) * pos_scale;
uint64_t raw_pos = operation.getData(motor_id, ADDR_PRESENT_POSITION, LEN_PRESENT_POSITION);
int64_t pos = unsigned_to_signed(raw_pos, 4);
_pos_data(index) = static_cast<float>(pos) * pos_scale;
}

Eigen::MatrixXd DynamixelPosReader::_get_data() {
Expand Down Expand Up @@ -788,9 +804,9 @@ void DynamixelVelReader::_initialize_data() {

void DynamixelVelReader::_update_data(int index, int motor_id) {
/* Updates the data index for the given motor ID */
uint32_t vel {operation.getData(motor_id, ADDR_PRESENT_VELOCITY, LEN_PRESENT_VELOCITY)};
vel = unsigned_to_signed(vel, 4);
_vel_data(index) = float(vel) * vel_scale;
uint64_t raw_vel = operation.getData(motor_id, ADDR_PRESENT_VELOCITY, LEN_PRESENT_VELOCITY);
int64_t vel = unsigned_to_signed(raw_vel, 4);
_vel_data(index) = static_cast<float>(vel) * vel_scale;
}

Eigen::MatrixXd DynamixelVelReader::_get_data() {
Expand Down Expand Up @@ -865,9 +881,9 @@ void DynamixelCurReader::_initialize_data() {

void DynamixelCurReader::_update_data(int index, int motor_id) {
/* Updates the data index for the given motor ID */
uint32_t cur {operation.getData(motor_id, ADDR_PRESENT_CURRENT, LEN_PRESENT_CURRENT)};
cur = unsigned_to_signed(cur, 2);
_cur_data(index) = float(cur) * cur_scale;
uint64_t raw_cur = operation.getData(motor_id, ADDR_PRESENT_CURRENT, LEN_PRESENT_CURRENT);
int64_t cur = unsigned_to_signed(raw_cur, 2);
_cur_data(index) = static_cast<float>(cur) * cur_scale;
}

Eigen::MatrixXd DynamixelCurReader::_get_data() {
Expand Down