Libdynamixel++ - A C++17 wrapper for the DynamixelSDK

Projects and Learning Showcase
1

Hi everyone :coffee: :wave:

Though a long-time on my road-map I only very recently got around to extract the custom written C++ wrapper around the Robotis Dynamixel SDK for the L3X-Z hexapod robot into its own C++17 library: libdynamixelplusplus.

One of the major advantages of C++17 from my perspective is automatic type unpacking and incredibly easy iteration over advanced C++ containers (such as std::map or a std::vector of std::tuple). In my humble opinion this allows a almost “functional programming” usage of C++ in terms of achievable code expressiveness.

The code snippet below demonstrates how the library can be used:

#include <cstdlib>
#include <iostream>
#include <dynamixel++/Dynamixel++.h>

using namespace dynamixelplusplus;

static uint16_t const MX28_ControlTable_Torque_Enable = 64;
static uint16_t const MX28_ControlTable_GoalPosition  = 116;

int main(int argc, char **argv)
{
  Dynamixel dynamixel_ctrl("/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT4NNZ55-if00-port0",
                           Dynamixel::Protocol::V2_0,
                           115200);

  Dynamixel::IdVect const id_vect{1,2,3,4};

  /* Synchronously enable torque for all servos. */
  std::map<Dynamixel::Id, uint8_t> torque_on_data_map;
  for (auto id : id_vect) torque_on_data_map[id] = 1;
  dynamixel_ctrl.syncWrite(MX28_ControlTable_Torque_Enable, torque_on_data_map);

  /* Synchronously set goal position for all servos. */
  std::map<Dynamixel::Id, uint32_t> goal_position_data_map;
  for (auto id : id_vect) goal_position_data_map[id] = (id - 1) * 1024;
  dynamixel_ctrl.syncWrite(MX28_ControlTable_Torque_Enable, goal_position_data_map);

  /* Read the current angle from all those servos and print it to std::cout. */
  std::map<Dynamixel::Id, uint32_t> position_map;
  auto err_read = dynamixel_ctrl.syncRead(MX28_ControlTable_PresentPosition, id_vect, position_map);
  if (err_read != Dynamixel::Error::None) {
    std::cerr << "'syncRead' failed with error code " << static_cast<int>(err_read) << std::endl;
    return EXIT_FAILURE;
  }
  
  for (auto [id, position_raw] : position_map)
  {
    float const position_deg = static_cast<float>(position_raw) * 360.0f / 4096;
    std::cout << "Dynamixel MX28 servo #"
              << static_cast<int>(id)
              << ": "
              << position_deg
              << " DEG"
              << std::endl;
  }

  return EXIT_SUCCESS;
}

Cheers, Alex

2 Likes
2

Would this wrapper work with the standard DXL SDK (ie Non-ROS)?

3

Good Morning :coffee: :wave:

This library is a wrapper around the standard DynamixelSDK - as written above :wink: .

You can both use it in stand-alone C++ applications (i.e. here) or within ROS code (i.e. done here). Or you can use the Robotis Dynamixel ROS wrapper, whichever way you prefer :bowing_man: .

Cheers, Alex

4

Hello everyone :coffee: :wave:

I just wanted to let you know that I’ve released v1.2.0 of libdynamixelplusplus. Turns out I’ve unfortunately forgot to announce the v.1.1.0 release here, hereby rectifying that neglect.

The main differences to 1.0.0 are that

  • normal read/write (single register, single servo)
  • sync read/write (multiple registers of same address, multiple servos) as well as
  • bulk read/write (multiple registers, multiple addresses, multiple servos)

are now all supported.

The API looks like this:

Read/Write

/* read */
uint8_t const firmware_version = dynamixel_ctrl.read<uint8_t>(MX28_ControlTable_Firmware_Version, id);
std::cout << "Servo #" << static_cast<int>(id)
          << " firmware version rev. " << static_cast<int>(firmware_version) << std::endl;
/* write */
uint8_t const LED_ON = 1;
dynamixel_ctrl.write(MX28_ControlTable_LED, id, LED_ON);

Sync Read

std::map<Dynamixel::Id, uint32_t> position_map = dynamixel_ctrl.syncRead<uint32_t>(MX28_ControlTable_PresentPosition, id_vect);

for (auto [id, position_raw] : position_map)
{
  float const position_deg = static_cast<float>(position_raw) * 360.0f / 4096;
  std::cout << "Dynamixel MX28 servo #"
            << static_cast<int>(id)
            << ": "
            << position_deg
/* ... */

Sync Write

std::map<Dynamixel::Id, uint8_t> led_on_data_map;

for (auto id : id_vect)
  led_on_data_map[id] = 1;

dynamixel_ctrl.syncWrite(MX28_ControlTable_LED, led_on_data_map);

Bulk Read

Dynamixel::BulkReadRequestVect bulk_read_req;

for (auto id : id_vect)
  bulk_read_req.push_back(std::make_tuple(id, MX28_ControlTable_PresentPosition, 

Dynamixel::BulkReadDataMap position_map ) = dynamixel_ctrl.bulkRead(bulk_read_req);

for (auto [id, position_raw] : position_map)
{
  float const position_deg = static_cast<float>(std::get<uint32_t>(position_raw)) * 360.0f / 4096;
  std::cout << "Dynamixel MX28 servo #"
            << static_cast<int>(id)
            << ": "
            << position_deg
/* ... */

Bulk Write

Dynamixel::BulkWriteDataVect led_on_data_vect;
uint8_t const LED_ON_VAL = 1;

for (auto id : id_vect)
  led_on_data_vect.push_back(std::make_tuple(id, MX28_ControlTable_LED, LED_ON_VAL));

dynamixel_ctrl.bulkWrite(led_on_data_vect);

If there are any questions, inputs, requests … please let me know :wink:

Cheers, Alex