User Tools

Site Tools


This is an old revision of the document!

Image Based Visual Servoing

This tutorial will show how to configure and program the Dynamixels servos in Cusing Visual Studio and the library provided by Dynamixel.Also , will show how to configure and program the Flea3 camera in C++ using Visual Studio and the library FlyCapture. ---- ===== USB2Dynamixel installation ===== First step is to check if your USB2dynamixel hardware is working good and recognizing the servos connected to it. Download the [[

 *Download Dynamixel SDK in the [[|github repository]]
 *Go to the subfolder: DynamixelSDK-master\c++\build\win64 and open the dxl_x64_cpp solution. Build the solution in Release and x64.  (this will create the dll in the output folder to be included in the envinment variables).

 *Open your Visual Studio and start a project where you already have the OpenCV included (see the OpenCV tutorials page to see how to include OpenCV into Visual Studio
 *Right click in your solution in the solution explorer and go to properties

 *Under C++ > general > additional include directories include the path : DynamixelSDK-master\c++\include ( depend on where you saved the github repository.
 *Under Linker > general > additional include directories include the path: DynamixelSDK-master\c++\build\win64\output ( depend on where you saved the github repository.
 *Under Linker > input > additional dependencies add the library dxl_x64_cpp.lib

 *Go to your file explorer , right click into your PC icon , and go to properties and click into advanced system settings

Click in environment variables and under system variables find the variable path and click in edit Include the path : DynamixelSDK-master\c\build\win64\output.

FlyCapture Library installation guide

  • Download the FlyCapture SDK
  • Install the SDK and open the FlyCapture software. Test and see if your Camera is recognized by your PC (REQUIRED USB 3.0). If it's everything OK you can continue the tutorial and include the library to work with C++
  • Open a solution in the Visual Studio where you already have OpenCV and Dynamixel SDK included.
  • Right click the solution in the solution explorer and go to properties.
  • Under C++ > general > additional include directories include the directory: Program Files\Point Grey\include
  • Under Linker > general > additional library directories include the directory: Program Files\Point Grey\lib64\vs2015
  • Under Link > input > additional dependencies include the library: FlyCapture2d_v140.lib

How to use Dynamixel SDK API's

The control of the dynamixel servos in C++ for the most applications is very straight forward. You will only read / write information through the serial port , and the information depends on the address that you put in the API.

Depending on your Dynamixel model , the address for certain properties can be different , so check before the address table of your dynamixel. For example , using the modelmx-64 this how the address table looks like:

The first column is the address that we will provide to the API , the second and third column is what this address represents , and the fourth column says what is the use ( Read (R) or write (W) or both (RW) ).

If we want to read the present position of any of the dynamixels connected to the USB2Dynamixel , this is done by using the API to read , providing address corresponding to present position and the Dynamixel ID .

Open the Visual Studio solution where you have the OpenCV and the Dynamixel SDK included. to make the SDK to work just include this lines into your code before the main function:

<Code> #ifdef linux #include <unistd.h> #include <fcntl.h> #include <termios.h> #elif defined(_WIN32) || defined(_WIN64) #include <conio.h> #endif

#include <Windows.h> #include “dynamixel_sdk.h”

using namespace FlyCapture2;

Protocol version #define PROTOCOL_VERSION 1.0 Default setting

#define BAUDRATE 1000000 Change in the Dyn. wizard all the servos baudrate to 100000 #define DEVICENAME “COM6” Port where the USB2Dynamixel is connected

int getch() { #ifdef linux

struct termios oldt, newt;
int ch;
tcgetattr(STDIN_FILENO, &oldt);
newt = oldt;
newt.c_lflag &= ~(ICANON | ECHO);
tcsetattr(STDIN_FILENO, TCSANOW, &newt);
ch = getchar();
tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
return ch;

#elif defined(_WIN32) || defined(_WIN64)

return _getch();

#endif }

int kbhit(void) { #ifdef linux

struct termios oldt, newt;
int ch;
int oldf;
tcgetattr(STDIN_FILENO, &oldt);
newt = oldt;
newt.c_lflag &= ~(ICANON | ECHO);
tcsetattr(STDIN_FILENO, TCSANOW, &newt);
oldf = fcntl(STDIN_FILENO, F_GETFL, 0);
ch = getchar();
tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
fcntl(STDIN_FILENO, F_SETFL, oldf);
if (ch != EOF)
  ungetc(ch, stdin);
  return 1;
return 0;

#elif defined(_WIN32) || defined(_WIN64)

return _kbhit();

#endif }


In order to start everything , include this into your main function.

<Code> Initialize PortHandler instance Set the port path

// Get methods and members of PortHandlerLinux or PortHandlerWindows
dynamixel::PortHandler *portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);
// Initialize PacketHandler instance
// Set the protocol version
// Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler
dynamixel::PacketHandler *packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION);
      int dxl_comm_result = COMM_TX_FAIL;             // Communication result
uint8_t dxl_error = 0;                          // Dynamixel error
      // Variables to get present position of the servos
uint16_t dxl_present_position1 = 0, dxl_present_position2 = 0, dxl_present_position3 = 0;
uint16_t dxl_present_position4 = 0, dxl_present_position5 = 0, dxl_present_position6 = 0;

Open port (USB2Dynamixel) if (portHandler→openPort()) { printf(“Succeeded to open the port!\n”); } else { printf(“Failed to open the port!\n”); printf(“Press any key to terminate…\n”); getch(); return 0; } Set port baudrate to 1000000

//All the dynamixels must be using baudrate 1000000 , configure this on the Dynamixel Wizard
if (portHandler->setBaudRate(BAUDRATE))
	printf("Succeeded to change the baudrate!\n");
	printf("Failed to change the baudrate!\n");
	printf("Press any key to terminate...\n");
	return 0;
// USB2Dynamixel successfully connected
printf("Dynamixel has been successfully connected \n");
printf("Press any Key to continue ");
std::cout << "" << std::endl;


This will open and configure the port to communicate with the USB2Dynamixel .

To read something (get information) from any dynamixel servo the API is:


dxl_comm_result = packetHandler→read2ByteTxRx(portHandler, 1, ADDR_MX_PRESENT_POSITION, &dxl_present_position1, &dxl_error);

float position = dxl_present_position1;


After the arrow we will define if we want to read or write. If we want to read , we use “read2ByteTxRx” after the packetHandler→ arrow. The first argument is already defined in our startup , the second argument is the dynamixel ID ( you can see which servo correspond to which id in the Dynamixel Wizard) , on this case we are reading an information from the dynamixel servo ID1. The third argument is what you want to read , note that the variable “ADDR_MX_PRESENT_POSITION” is defined as 36 , if we check on the address table from the dynamixel , the address 36 correspond to present position , so we are reading the present position from dynamixel ID1. The fourth argument will hold the result from the reading - the present position - and the last argument will hold an error information (depending on its value we can know if the reading was successful or not).

If we want to write something (send information) the idea is very similar to the reading:

<Code> int ADDR_MX_GOAL_POSITION=30; int gripper=560;

dxl_comm_result = packetHandler→write2ByteTxRx(portHandler, 6, ADDR_MX_GOAL_POSITION, gripper, &dxl_error);


What is changed here is after the arrow from the packetHandler → we use “write2ByteTxRx” . The arguments is the same from the reading example. On this case we are sending information to dynamixel ID6, the information is the address 30 , which corresponds to add goal position (move the servo to this position ) , the gripper variable now instead of hold a information ( reading) it will send a information , so the value 560 is the goal position send to the dynamixel ID6.

Moving the Dynamixel in X degrees

The dynamixels operates with goal positions. The relation between goal position and angle in degrees can be found in the same document as the Address table. For example , for the MX-64 we can check in the documentation that the resolution is 0.088. In other words , 1 goal position corresponds to 0.088 degrees. If we want to move 10 degrees , we need to send the information as GP = angle in degrees / resolution , which is , GP=10/0.088

How to get Flea3 Camera feed and convert to OpenCV format

In order to work with the camera feed provided by the FlyCapture library we need to convert the image to the Mat format - format that OpenCV function uses.

To start and get the camera feed we use the code:

<Code> Variables to be used on the Flea3 Camera Error error; Camera camera; CameraInfo camInfo; Connect the Flea3 camera

error = camera.Connect(0);
if (error != PGRERROR_OK)
	std::cout << "Failed to connect to camera" << std::endl;
	return false;
// Get the camera info and print it out
error = camera.GetCameraInfo(&camInfo);
if (error != PGRERROR_OK)
	std::cout << "Failed to get camera info from camera" << std::endl;
	return false;
std::cout << camInfo.vendorName << " "
	<< camInfo.modelName << " "
	<< camInfo.serialNumber << std::endl;
error = camera.StartCapture();
	std::cout << "Bandwidth exceeded" << std::endl;
	return false;
else if (error != PGRERROR_OK)
	std::cout << "Failed to start image capture" << std::endl;
	return false;
std::cout << " Flea3 Camera Connected " << std::endl;
std::wcout << "Press any key to continue " << std::endl;


This will start the Flea3 Camera and check if the connection is working well. To get the camera feed and convert to Mat type we use:


                         // Get Flea3 camera image
			Image rawImage;
			Error error = camera.RetrieveBuffer(&rawImage);
                         // convert to rgb
			Image rgbImage;
			rawImage.Convert(FlyCapture2::PIXEL_FORMAT_BGR, &rgbImage);
		// convert to OpenCV Mat 
unsigned int rowBytes = (double)rgbImage.GetReceivedDataSize() / (double)rgbImage.GetRows();
cv::Mat coloredimage = cv::Mat(rgbImage.GetRows(), rgbImage.GetCols(), CV_8UC3, rgbImage.GetData(), rowBytes);
//Resize the image to 640 x 480 
cv::resize(coloredimage, coloredimage, cv::Size(640, 480));


This code will create a Mat variable called coloredimage , using RGB channel , to be used in the OpenCV functions.

robotic_manipulators_ibvs.1470707331.txt.gz · Last modified: 2016/08/08 18:48 by joaomatos