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

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

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

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