6 September 2011

Using dynamixel motors in ROS in Ubuntu 10.10

The Dynamixel AX-12 Servo from Robotis is one of the advanced actuator on the market, in this price range. It is widely used in hobby robotics. The Dynamixl AX-12 robot servo has the ability to track its speed, temperature, shaft position, voltage, and load. These parameters are coded on 10 bits each. Also, the algoirthm used to maintain shaft position on the AX-12 actuator can be adjuste individually for each servo, allowing you to control the speed and strength of the motor's response. All of the sensor management and position control is handled by the servo's build-in microcontroller (ATMega 8). The Dynamixel AX-12 servos are different from regular hobby servos. They connect to a serial bus, and each has an ID number.

The above is the picture shows how a Dynamixel AX-12 motor looks like. The figure below shows the pin configuration and has to be kept in mind while connecting it to power source or to other motors.
I am using 3 Dynamixel AX-12 serially connected as daisy chain and is interfaced with the computer using USB2Dyanamixel. This device also has its SDK in C so that the motor movements and values could be easily accessed from ROS nodes. The figure below is of the USB2Dynamixel dongle device.
I am working on Ubuntu 10.10 with 2.6.35-28-generic kernel version. Its recommended by Dynamixel SDK providers to use kernel greater than 2.6.21 as the code regarding FT232RL is already added to the kernel code so you don't have to install this. 

Just download the Dynamixel SDK for linux from here.

The SDK for linux folder composes of the following folder after making it -
/include  :this contains header files to use Dynamixel SDK
/src         :this folder contains the Dynamixel SDK source files
/lib          :this contains the compiled library files
/example :there are examples to control Dynamixel motors

I copied the dynamixel.h header file into standard header folder i.e. /usr/include and libdxl.a library into /usr/lib so that it may work as any linux shared library. After doing all this, the system is ready to interact with the motors using the C code. The motors must be connected to the computer using USB2Dynamixel connected to any of the USB port.

Before going to the coding, its better to go through the AX-12 manual and also through the Dynamixel APIs in C. The address values and pin configurations should be seen as it will be required while programming to know the addresses. This can be accessed from here

Here is a sample program which I used for rotating three motors connected serially.

#include <stdio.h>
#include <termio.h>
#include <unistd.h>
#include <dynamixel.h>
#include <termios.h>

int getch(void)
  int ch;
  struct termios oldt;
  struct termios newt;
  tcgetattr(STDIN_FILENO, &oldt); /*store old settings */
  newt = oldt; /* copy old settings to new settings */
  newt.c_lflag &= ~(ICANON | ECHO); /* make one change to old settings in new settings */
  tcsetattr(STDIN_FILENO, TCSANOW, &newt); /*apply the new settings immediatly */
  ch = getchar(); /* standard getchar call */
  tcsetattr(STDIN_FILENO, TCSANOW, &oldt); /*reapply the old settings */
  return ch; /*return received char */

// Control table address
#define P_GOAL_POSITION_L 30
#define P_GOAL_POSITION_H 31
#define P_MOVING 46

/** Motor IDs of all three motors motors */
#define MOTOR1      18
#define MOTOR2      17
#define MOTOR3      15

// Id of the currently connected dynamixel
#define DEFAULT_BAUDNUM 1 // 1Mbps
#define DEFAULT_ID 15

void PrintCommStatus(int CommStatus);
void PrintErrorCode(void);

int main()
        int baudnum = 1;
         * One digit of motor represents 0.3515625 degrees.
         * One degree represents 2.8444444 values of motor turn
        int GoalPos[2] = {512, 512};
         * Middle Motor limits - [ -92.109, 101.25  ]
        int GoalPosMotor2[2] = { 250, 800 };
         * Base Motor limits - [-180, 180 ];
        int GoalPosMotor1[2] = { 0, 1024 };
         * Top Motor limits - [ 77.344, 125.859 ];
        int GoalPosMotor3[2] = { 292, 870 };
        //int GoalPos[2] = {0, 4095}; // for Ex series
        int index = 0;
        int deviceIndex = 0;
        int Moving, PresentPos;
        int CommStatus;

        printf( "\n\nRead/Write example for Linux\n\n" );
        ///////// Open USB2Dynamixel ////////////
        if( dxl_initialize(deviceIndex, baudnum) == 0 )
                printf( "Failed to open USB2Dynamixel!\n" );
                printf( "Press Enter key to terminate...\n" );
                return 0;
                printf( "Succeed to open USB2Dynamixel!\n" );
        int turnValue = 512;
        dxl_write_word( 18, 32, 100 );
        dxl_write_word( 17, 32, 100 );
        dxl_write_word( 15, 32, 100 );
        dxl_write_word( BROADCAST_ID, P_GOAL_POSITION_L, turnValue );

                printf( "Press Enter key to continue!(press ESC and Enter to quit)\n" );
                char c = getch();
                if(c == 0x1b)
                if(c == 'a')
                if(c == 'd')

                 printf("Turn value - %d\n", turnValue);
                // Write goal position

                dxl_write_word( 18, P_GOAL_POSITION_L, 900 );
                dxl_write_word( 17, P_GOAL_POSITION_L, 400 );
                dxl_write_word( 15, P_GOAL_POSITION_L, 700 );
                        // Read present position
                        PresentPos = dxl_read_word( DEFAULT_ID, P_PRESENT_POSITION_L );
                        CommStatus = dxl_get_result();

                        if( CommStatus == COMM_RXSUCCESS )
                                printf( "%d   %d\n",GoalPos[index], PresentPos );

                        // Check moving done
                        Moving = dxl_read_byte( DEFAULT_ID, P_MOVING );
                        CommStatus = dxl_get_result();
                        if( CommStatus == COMM_RXSUCCESS )
                                if( Moving == 0 )
                                        // Change goal position
                                        if( index == 0 )
                                                index = 1;
                                                index = 0;

                }while(Moving == 1);

        // Close device
        printf( "Press Enter key to terminate...\n" );
        return 0;

/** Printing communication result */
void PrintCommStatus(int CommStatus)
        case COMM_TXFAIL:
                printf("COMM_TXFAIL: Failed transmit instruction packet!\n");

        case COMM_TXERROR:
                printf("COMM_TXERROR: Incorrect instruction packet!\n");

        case COMM_RXFAIL:
                printf("COMM_RXFAIL: Failed get status packet from device!\n");

        case COMM_RXWAITING:
                printf("COMM_RXWAITING: Now recieving status packet!\n");

        case COMM_RXTIMEOUT:
                printf("COMM_RXTIMEOUT: There is no status packet!\n");

        case COMM_RXCORRUPT:
                printf("COMM_RXCORRUPT: Incorrect status packet!\n");

                printf("This is unknown error code!\n");

/** Print error bit of status packet */
void PrintErrorCode()
        if(dxl_get_rxpacket_error(ERRBIT_VOLTAGE) == 1)
                printf("Input voltage error!\n");

        if(dxl_get_rxpacket_error(ERRBIT_ANGLE) == 1)
                printf("Angle limit error!\n");

        if(dxl_get_rxpacket_error(ERRBIT_OVERHEAT) == 1)
                printf("Overheat error!\n");

        if(dxl_get_rxpacket_error(ERRBIT_RANGE) == 1)
                printf("Out of range error!\n");

        if(dxl_get_rxpacket_error(ERRBIT_CHECKSUM) == 1)
                printf("Checksum error!\n");

        if(dxl_get_rxpacket_error(ERRBIT_OVERLOAD) == 1)
                printf("Overload error!\n");

        if(dxl_get_rxpacket_error(ERRBIT_INSTRUCTION) == 1)
                printf("Instruction code error!\n");

This program can be compiled using the following line - 
$ g++ main.cpp -o output -ldxl -lm

For integrating it with ros, we just need to add the library in the CMakeLists.txt as -
target_link_libraries(<executable name> -ldxl -lm)

References - 

No comments:

Post a Comment