From 52e16f79c89e29abf64a201026f5c97b3812125b Mon Sep 17 00:00:00 2001 From: Dorian Goepp Date: Tue, 2 Apr 2019 21:18:59 +0200 Subject: [PATCH 1/3] restore and rename the new version of set_goal_positions --- src/dynamixel/servos/servo.hpp | 28 +++++++++++++++++++++------- 1 file changed, 21 insertions(+), 7 deletions(-) diff --git a/src/dynamixel/servos/servo.hpp b/src/dynamixel/servos/servo.hpp index 02990f60..36d4a9d2 100644 --- a/src/dynamixel/servos/servo.hpp +++ b/src/dynamixel/servos/servo.hpp @@ -229,17 +229,31 @@ namespace dynamixel { // Sync operations. Only works if the models are known and they are all the same // use case : std::make_shared(0)->set_goal_positions(ids, angles)); replace MODEL_SERVO by Mx28 or Xl320... + template + static InstructionPacket set_goal_positions_angle(const std::vector& ids, const std::vector& positions) + { + if (ids.size() != positions.size()) + throw errors::Error("Instruction: error when setting goal positions: \n\tMismatch in vector size for ids and positions"); + // Convert from radians to ticks (after going through degrees) + std::vector tick_positions; + for (auto pos : positions) + tick_positions.push_back((((pos * 57.2958) - ct_t::min_goal_angle_deg) * (ct_t::max_goal_position - ct_t::min_goal_position) / (ct_t::max_goal_angle_deg - ct_t::min_goal_angle_deg)) + ct_t::min_goal_position); + // Pack the angles into bytes (for sending) + std::vector> packed(tick_positions.size()); + for (auto tick_pos : tick_positions) + packed.push_back(protocol_t::pack_data((typename ct_t::goal_position_t)tick_pos)); + + return sync_write_t(ct_t::goal_position, _get_typed(ids), packed); + } + template static InstructionPacket set_goal_positions(const std::vector& ids, const std::vector& pos) { - std::vector final_pos; - for (size_t j = 0; j < pos.size(); j++) - final_pos.push_back((((pos[j] * 57.2958) - ct_t::min_goal_angle_deg) * (ct_t::max_goal_position - ct_t::min_goal_position) / (ct_t::max_goal_angle_deg - ct_t::min_goal_angle_deg)) + ct_t::min_goal_position); - if (ids.size() != final_pos.size()) + if (ids.size() != pos.size()) throw errors::Error("Instruction: error when setting goal positions: \n\tMismatch in vector size for ids and positions"); - std::vector> packed(final_pos.size()); - for (size_t i = 0; i < final_pos.size(); i++) - packed[i] = protocol_t::pack_data((typename ct_t::goal_position_t)final_pos[i]); + std::vector> packed(pos.size()); + for (size_t i = 0; i < pos.size(); i++) + packed[i] = protocol_t::pack_data((typename ct_t::goal_position_t)pos[i]); return sync_write_t(ct_t::goal_position, _get_typed(ids), packed); } From ca8b2a5fc0c0eb3b08a0686b49df7921c1d73391 Mon Sep 17 00:00:00 2001 From: Dorian Goepp Date: Sat, 6 Apr 2019 14:44:07 +0200 Subject: [PATCH 2/3] use angle version of set_goal_positions --- src/tools/utility.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/tools/utility.hpp b/src/tools/utility.hpp index fd485320..c4d3685e 100644 --- a/src/tools/utility.hpp +++ b/src/tools/utility.hpp @@ -434,7 +434,7 @@ namespace dynamixel { "vectors of IDs and angles should have " "the same length"); _serial_interface.send( - std::make_shared(0)->set_goal_positions(ids, angles)); //Mx28 + std::make_shared(0)->set_goal_positions_angle(ids, angles)); //Mx28 StatusPacket status; for (int i = 0; i < ids.size(); i++) { From 380319afa4bb3b03db41c06d60141d8805989bf3 Mon Sep 17 00:00:00 2001 From: Dorian Goepp Date: Sat, 6 Apr 2019 16:27:41 +0200 Subject: [PATCH 3/3] Add two error types for vector sizes --- src/dynamixel/errors/bad_packet.hpp | 6 +-- src/dynamixel/errors/crc_error.hpp | 11 +++-- src/dynamixel/errors/error.hpp | 4 +- src/dynamixel/errors/servo_limit_error.hpp | 8 +-- src/dynamixel/errors/status_error.hpp | 6 +-- src/dynamixel/errors/unpack_error.hpp | 4 +- src/dynamixel/errors/vector_size_errors.hpp | 55 +++++++++++++++++++++ src/dynamixel/instructions/bulk_read.hpp | 12 ++--- src/dynamixel/instructions/bulk_write.hpp | 4 +- src/dynamixel/instructions/sync_read.hpp | 4 +- src/dynamixel/instructions/sync_write.hpp | 10 ++-- src/dynamixel/servos/servo.hpp | 23 ++++----- 12 files changed, 102 insertions(+), 45 deletions(-) create mode 100644 src/dynamixel/errors/vector_size_errors.hpp diff --git a/src/dynamixel/errors/bad_packet.hpp b/src/dynamixel/errors/bad_packet.hpp index c45995e2..c17b03b7 100644 --- a/src/dynamixel/errors/bad_packet.hpp +++ b/src/dynamixel/errors/bad_packet.hpp @@ -1,8 +1,8 @@ #ifndef DYNAMIXEL_BAD_PACKET_ERROR_HPP_ #define DYNAMIXEL_BAD_PACKET_ERROR_HPP_ -#include #include +#include #include "error.hpp" @@ -39,7 +39,7 @@ namespace dynamixel { private: const std::vector _packet; }; - } -} + } // namespace errors +} // namespace dynamixel #endif diff --git a/src/dynamixel/errors/crc_error.hpp b/src/dynamixel/errors/crc_error.hpp index 6be126ee..2cc3cb42 100644 --- a/src/dynamixel/errors/crc_error.hpp +++ b/src/dynamixel/errors/crc_error.hpp @@ -1,9 +1,9 @@ #ifndef DYNAMIXEL_ERRORS_CRC_ERROR_HPP_ #define DYNAMIXEL_ERRORS_CRC_ERROR_HPP_ -#include -#include #include +#include +#include #include "error.hpp" @@ -11,7 +11,8 @@ namespace dynamixel { namespace errors { class CrcError : public Error { public: - CrcError(uint8_t id, uint8_t protocol, uint32_t expected, uint32_t received) : _id(id), _protocol(protocol), _expected(expected), _received(received) + CrcError(uint8_t id, uint8_t protocol, uint32_t expected, uint32_t received) + : _id(id), _protocol(protocol), _expected(expected), _received(received) { std::stringstream err_message; err_message << "Status: checksum error while decoding packet with ID " << (int)id; @@ -44,7 +45,7 @@ namespace dynamixel { uint8_t _id, _protocol; uint32_t _expected, _received; }; - } -} + } // namespace errors +} // namespace dynamixel #endif diff --git a/src/dynamixel/errors/error.hpp b/src/dynamixel/errors/error.hpp index 70af8bd1..044ebdbe 100644 --- a/src/dynamixel/errors/error.hpp +++ b/src/dynamixel/errors/error.hpp @@ -47,8 +47,8 @@ namespace dynamixel { { return err.print(os); } - } -} + } // namespace errors +} // namespace dynamixel #define CHECK(val, msg) check(__FILE__, __LINE__, (val), msg) #endif diff --git a/src/dynamixel/errors/servo_limit_error.hpp b/src/dynamixel/errors/servo_limit_error.hpp index 5ae063c1..633e698d 100644 --- a/src/dynamixel/errors/servo_limit_error.hpp +++ b/src/dynamixel/errors/servo_limit_error.hpp @@ -1,9 +1,9 @@ #ifndef DYNAMIXEL_ERRORS_SERVO_LIMIT_ERROR_HPP_ #define DYNAMIXEL_ERRORS_SERVO_LIMIT_ERROR_HPP_ -#include -#include #include +#include +#include #include "error.hpp" @@ -46,7 +46,7 @@ namespace dynamixel { int _id; double _max, _min, _value; }; - } -} + } // namespace errors +} // namespace dynamixel #endif diff --git a/src/dynamixel/errors/status_error.hpp b/src/dynamixel/errors/status_error.hpp index 8fa074d6..83c605cd 100644 --- a/src/dynamixel/errors/status_error.hpp +++ b/src/dynamixel/errors/status_error.hpp @@ -1,8 +1,8 @@ #ifndef DYNAMIXEL_STATUS_ERROR_HPP_ #define DYNAMIXEL_STATUS_ERROR_HPP_ -#include #include +#include #include "error.hpp" @@ -38,7 +38,7 @@ namespace dynamixel { private: uint8_t _id, _protocol, _error_byte; }; - } -} + } // namespace errors +} // namespace dynamixel #endif diff --git a/src/dynamixel/errors/unpack_error.hpp b/src/dynamixel/errors/unpack_error.hpp index 0c0d0df5..f4411765 100644 --- a/src/dynamixel/errors/unpack_error.hpp +++ b/src/dynamixel/errors/unpack_error.hpp @@ -1,9 +1,9 @@ #ifndef DYNAMIXEL_ERRORS_UNPACK_ERROR_HPP_ #define DYNAMIXEL_ERRORS_UNPACK_ERROR_HPP_ -#include -#include #include +#include +#include #include "error.hpp" diff --git a/src/dynamixel/errors/vector_size_errors.hpp b/src/dynamixel/errors/vector_size_errors.hpp new file mode 100644 index 00000000..cd655bc4 --- /dev/null +++ b/src/dynamixel/errors/vector_size_errors.hpp @@ -0,0 +1,55 @@ +#ifndef DYNAMIXEL_VECTOR_SIZE_ERROR_HPP_ +#define DYNAMIXEL_VECTOR_SIZE_ERROR_HPP_ + +#include + +#include "error.hpp" + +namespace dynamixel { + namespace errors { + class VectorEmptyError : public Error { + public: + VectorEmptyError(const std::string& method_name, + const std::string& vector_name) + : _method_name(method_name), _vector_name(vector_name) + { + this->_msg = "The vector " + vector_name + ", passed to " + + method_name + " cannot be empty (size 0)"; + } + + std::string vector_name() const + { + return _vector_name; + } + + std::string method_name() const + { + return _method_name; + } + + protected: + const std::string _method_name, _vector_name; + }; + + class VectorSizesDifferError : public Error { + public: + VectorSizesDifferError(const std::string& method_name, + const std::string& name1, const std::string& name2) + : _method_name(method_name) + { + this->_msg = "The vectors " + name1 + " and " + name2 + + ", passed to " + method_name + " should have the same size."; + } + + std::string method_name() const + { + return _method_name; + } + + protected: + const std::string _method_name; + }; + } // namespace errors +} // namespace dynamixel + +#endif diff --git a/src/dynamixel/instructions/bulk_read.hpp b/src/dynamixel/instructions/bulk_read.hpp index 5d150a3f..edd52dce 100644 --- a/src/dynamixel/instructions/bulk_read.hpp +++ b/src/dynamixel/instructions/bulk_read.hpp @@ -4,7 +4,7 @@ #include #include -#include "../errors/error.hpp" +#include "../errors/vector_size_errors.hpp" #include "../instruction_packet.hpp" namespace dynamixel { @@ -54,7 +54,7 @@ namespace dynamixel { const uint8_t& data_length) { if (ids.size() == 0) - throw errors::Error("BulkRead: ids vector of size zero"); + throw errors::VectorEmptyError("BulkRead", "ids"); std::vector parameters(3 * ids.size() + 3); @@ -95,7 +95,7 @@ namespace dynamixel { const uint16_t& data_length) { if (ids.size() == 0) - throw errors::Error("BulkRead: ids vector of size zero"); + throw errors::VectorEmptyError("BulkRead", "ids"); std::vector parameters(5 * ids.size() + 5); size_t curr = 0; @@ -116,11 +116,11 @@ namespace dynamixel { std::vector lengths) { if (ids.size() == 0) - throw errors::Error("BulkRead: ids vector of size zero"); + throw errors::VectorEmptyError("BulkRead", "ids"); if (ids.size() != addresses.size()) - throw errors::Error("BulkRead: mismatching size for ids and addresses"); + throw errors::VectorSizesDifferError("BulkRead", "ids", "addresses"); if (ids.size() != lengths.size()) - throw errors::Error("BulkRead: mismatching size for ids and lengths/sizes"); + throw errors::VectorSizesDifferError("BulkRead", "ids", "lengths"); } }; } // namespace instructions diff --git a/src/dynamixel/instructions/bulk_write.hpp b/src/dynamixel/instructions/bulk_write.hpp index ad706dce..052d7f50 100644 --- a/src/dynamixel/instructions/bulk_write.hpp +++ b/src/dynamixel/instructions/bulk_write.hpp @@ -4,7 +4,7 @@ #include #include -#include "../errors/error.hpp" +#include "../errors/vector_size_errors.hpp" #include "../instruction_packet.hpp" namespace dynamixel { @@ -22,7 +22,7 @@ namespace dynamixel { const uint16_t& data_length, const std::vector>& data) { if (ids.size() == 0) - throw errors::Error("BulkWrite: ids vector of size zero"); + throw errors::VectorEmptyError("BulkWrite", "ids"); std::vector parameters((5 + data_length) * ids.size() + 5); size_t curr = 0; diff --git a/src/dynamixel/instructions/sync_read.hpp b/src/dynamixel/instructions/sync_read.hpp index b1ed889f..e3e450a6 100644 --- a/src/dynamixel/instructions/sync_read.hpp +++ b/src/dynamixel/instructions/sync_read.hpp @@ -4,7 +4,7 @@ #include #include -#include "../errors/error.hpp" +#include "../errors/vector_size_errors.hpp" #include "../instruction_packet.hpp" namespace dynamixel { @@ -20,7 +20,7 @@ namespace dynamixel { uint16_t data_length) { if (ids.size() == 0) - throw errors::Error("SyncRead: ids vector of size zero"); + throw errors::VectorEmptyError("SyncRead", "ids"); std::vector parameters(ids.size() + 9); diff --git a/src/dynamixel/instructions/sync_write.hpp b/src/dynamixel/instructions/sync_write.hpp index 187e293c..2ef73e6e 100644 --- a/src/dynamixel/instructions/sync_write.hpp +++ b/src/dynamixel/instructions/sync_write.hpp @@ -4,7 +4,7 @@ #include #include -#include "../errors/error.hpp" +#include "../errors/vector_size_errors.hpp" #include "../instruction_packet.hpp" namespace dynamixel { @@ -21,9 +21,9 @@ namespace dynamixel { const std::vector>& data) { if (ids.size() == 0) - throw errors::Error("SyncWrite: ids vector of size zero"); + throw errors::VectorEmptyError("SyncWrite", "ids"); if (ids.size() != data.size()) - throw errors::Error("SyncWrite: mismatching vectors size for ids and data"); + throw errors::VectorSizesDifferError("SyncWrite", "ids", "data"); typename T::length_t data_length = data[0].size(); std::vector parameters((data_length + 1) * ids.size() + 2); @@ -50,9 +50,9 @@ namespace dynamixel { const std::vector>& data) { if (ids.size() == 0) - throw errors::Error("SyncWrite: ids vector of size zero"); + throw errors::VectorEmptyError("SyncWrite", "ids"); if (ids.size() != data.size()) - throw errors::Error("SyncWrite: mismatching vectors size for ids and data"); + throw errors::VectorSizesDifferError("SyncWrite", "ids", "data"); typename T::length_t data_length = data[0].size(); std::vector parameters((data_length + 1) * ids.size() + 4); diff --git a/src/dynamixel/servos/servo.hpp b/src/dynamixel/servos/servo.hpp index 36d4a9d2..dd4ecf90 100644 --- a/src/dynamixel/servos/servo.hpp +++ b/src/dynamixel/servos/servo.hpp @@ -6,6 +6,7 @@ #include "../errors/error.hpp" #include "../errors/servo_limit_error.hpp" +#include "../errors/vector_size_errors.hpp" #include "../instruction_packet.hpp" #include "../instructions/action.hpp" #include "../instructions/bulk_read.hpp" @@ -230,17 +231,17 @@ namespace dynamixel { // Sync operations. Only works if the models are known and they are all the same // use case : std::make_shared(0)->set_goal_positions(ids, angles)); replace MODEL_SERVO by Mx28 or Xl320... template - static InstructionPacket set_goal_positions_angle(const std::vector& ids, const std::vector& positions) + static InstructionPacket set_goal_positions_angle(const std::vector& ids, const std::vector& angles) { - if (ids.size() != positions.size()) - throw errors::Error("Instruction: error when setting goal positions: \n\tMismatch in vector size for ids and positions"); + if (ids.size() != angles.size()) + throw errors::VectorSizesDifferError("set_goal_positions_angle", "ids", "angles"); // Convert from radians to ticks (after going through degrees) - std::vector tick_positions; - for (auto pos : positions) - tick_positions.push_back((((pos * 57.2958) - ct_t::min_goal_angle_deg) * (ct_t::max_goal_position - ct_t::min_goal_position) / (ct_t::max_goal_angle_deg - ct_t::min_goal_angle_deg)) + ct_t::min_goal_position); + std::vector positions; + for (auto pos : angles) + positions.push_back((((pos * 57.2958) - ct_t::min_goal_angle_deg) * (ct_t::max_goal_position - ct_t::min_goal_position) / (ct_t::max_goal_angle_deg - ct_t::min_goal_angle_deg)) + ct_t::min_goal_position); // Pack the angles into bytes (for sending) - std::vector> packed(tick_positions.size()); - for (auto tick_pos : tick_positions) + std::vector> packed(positions.size()); + for (auto tick_pos : positions) packed.push_back(protocol_t::pack_data((typename ct_t::goal_position_t)tick_pos)); return sync_write_t(ct_t::goal_position, _get_typed(ids), packed); @@ -250,7 +251,7 @@ namespace dynamixel { static InstructionPacket set_goal_positions(const std::vector& ids, const std::vector& pos) { if (ids.size() != pos.size()) - throw errors::Error("Instruction: error when setting goal positions: \n\tMismatch in vector size for ids and positions"); + throw errors::VectorSizesDifferError("set_goal_positions", "ids", "pos"); std::vector> packed(pos.size()); for (size_t i = 0; i < pos.size(); i++) packed[i] = protocol_t::pack_data((typename ct_t::goal_position_t)pos[i]); @@ -352,7 +353,7 @@ namespace dynamixel { static InstructionPacket set_moving_speeds(const std::vector& ids, const std::vector& speeds, OperatingMode operating_mode) { if (ids.size() != speeds.size()) - throw errors::Error("Instruction: error when setting moving speeds: \n\tMismatch in vector size for ids and speeds"); + throw errors::VectorSizesDifferError("set_moving_speeds", "ids", "speeds"); std::vector speed_ticks; // convert radians per second to ticks for (int i = 0; i < speeds.size(); i++) @@ -434,7 +435,7 @@ namespace dynamixel { static InstructionPacket set_torque_limits(const std::vector& ids, const std::vector& torque_limits) { if (ids.size() != torque_limits.size()) - throw errors::Error("Instruction: error when setting torque limits: \n\tMismatch in vector size for ids and torques"); + throw errors::VectorSizesDifferError("set_torque_limits", "ids", "torque_limits"); std::vector> packed(torque_limits.size()); for (size_t i = 0; i < torque_limits.size(); i++) packed[i] = protocol_t::pack_data((typename ct_t::torque_limit_t)torque_limits[i]);