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 9632912e..74a96232 100644 --- a/src/dynamixel/servos/servo.hpp +++ b/src/dynamixel/servos/servo.hpp @@ -3,6 +3,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" @@ -245,6 +246,96 @@ namespace dynamixel { return Model::parse_present_position_angle(this->_id, st); } +<<<<<<< HEAD + // static InstructionPacket::protocol_t> get_current_positions_all(typename Servo::protocol_t::id_t id, const std::vector ids) + // { + // typename Servo::ct_t::present_position_t pos; + // std::vector address; + // std::vector data_length; + // for (size_t i = 0; i < ids.size(); i++) { + // address.push_back(pos); // adress 36 on MX models (0x24) -- adress 37 on XL models (0x25) + // data_length.push_back(sizeof(pos)); + // } + // return bulk_read_t(_get_typed(ids), address, data_length); + // } + // + // InstructionPacket::protocol_t> get_current_positions_all(const std::vector ids) const override + // { + // return Model::get_current_positions_all(this->_id, ids); + // } + + // 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& angles) + { + 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 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(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); + } + + template + static InstructionPacket set_goal_positions(const std::vector& ids, const std::vector& pos) + { + if (ids.size() != pos.size()) + 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]); + + return sync_write_t(ct_t::goal_position, _get_typed(ids), packed); + } + + // Bulk operations. Only works for MX models with protocol 1. Only works if the models are known and they are all the same + template + static InstructionPacket get_current_positions_MX(const std::vector& ids) + { + std::vector address; + std::vector data_length; + for (size_t i = 0; i < ids.size(); i++) { + address.push_back(0x24); // adress 36 on MX models (0x24) -- adress 37 on XL models (0x25) + data_length.push_back(0x02); + } + return bulk_read_t(_get_typed(ids), address, data_length); + } + + // Bulk operations. Only works for XL models with protocol 2. Only works if the models are known and they are all the same + template + static InstructionPacket get_current_positions_XL(const std::vector& ids) + { + std::vector address; + std::vector data_length; + for (size_t i = 0; i < ids.size(); i++) { + address.push_back(0x25); // adress 36 on MX models (0x24) -- adress 37 on XL models (0x25) + data_length.push_back(0x02); + } + return bulk_read_t(_get_typed(ids), address, data_length); + } + + // Bulk operations. Only works for XM models with protocol 2. Only works if the models are known and they are all the same + template + static InstructionPacket get_current_positions_XM(const std::vector& ids) + { + std::vector address; + std::vector data_length; + for (size_t i = 0; i < ids.size(); i++) { + address.push_back(0x84); // adress 36 on MX models (0x24) -- adress 37 on XL models (0x25) + data_length.push_back(0x04); + } + return bulk_read_t(_get_typed(ids), address, data_length); + } + +======= +>>>>>>> dev // ================================================================= // Speed-specific @@ -344,7 +435,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++) @@ -405,7 +496,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]); diff --git a/src/tools/utility.hpp b/src/tools/utility.hpp index b3476132..86d23f3f 100644 --- a/src/tools/utility.hpp +++ b/src/tools/utility.hpp @@ -433,6 +433,11 @@ namespace dynamixel { throw errors::UtilityError("set_position(vector, vector): the " "vectors of IDs and angles should have " "the same length"); +<<<<<<< HEAD + _serial_interface.send( + std::make_shared(0)->set_goal_positions_angle(ids, angles)); //Mx28 + +======= std::vector ids_cast; for (int i = 0; i < ids.size(); i++) { ids_cast.push_back((uint8_t)ids[i]); @@ -440,6 +445,7 @@ namespace dynamixel { // _serial_interface.send( // std::make_shared(0)->set_goal_positions(ids, angles)); //Mx28 _serial_interface.send(_servos.at(ids[0])->sync_goal_position_angle(ids_cast, angles)); +>>>>>>> dev StatusPacket status; for (int i = 0; i < ids.size(); i++) { _serial_interface.recv(status);