DASL Wiki

This page is read only. You can view the source, but not change it. Ask your administrator if you think this is wrong.
===== Image Based Visual Servoing =====

**Author:** Joao Matos Email: <jcunha@id.uff.br> <!-- replace with your email address -->
\\
**Date:** Last modified on 6/8/2016
\\
**Keywords:** Visual Servoing , Dynamixel SDK, Flea3 Camera, FlyCapture
\\

 This tutorial will show how to configure and program the Dynamixels servos in Cpp using Visual Studio and the library provided by Dynamixel.Also , will show how to configure and program the Flea3 camera in Cpp 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 [[http://www.robotis.com/xe/download_en|RoboPlus]] software provided by Robotis and install in your PC. 

 After installing , open the RoboPLus , and go to Expert tab and open the Dynamixel Wizard.

{{ ::ii7.jpg?direct |}}

 Select the port where the USB2dynamixel is connected and open the port. 

{{ ::ii8.jpg?direct |}}

 Select your dynamixels protocol version (1 or 2) , if you don't know just search 1 and after 2 and see what dynamixel use what protocol.

{{ ::dt1.jpg?direct |}}
 
If is everything working you will see a window like this shown bellow. This is where you will configure your servos (moving speed , torque limit , angle limit , etc... ). See the dynamixel manual to see what is all the configuration possibilities to your servo

{{ ::dt2.jpg?direct |}}

The addr column can be useful if you want to know the address to access some information ( later on this tutorial we will need to read / write information on the servo using this address ). Also , the USB2Dynamixel can only communicate with dynamixels using the same baudrate , so before you start coding , is important to change the baudrate to 1000000 on every dynamixel found by the Dynamixel wizard.

----

===== Dynamixel Library installation guide =====

   *Download Dynamixel SDK in the [[https://github.com/ROBOTIS-GIT/DynamixelSDK/tree/master/c%2B%2B|github repository]]
   *Go to the subfolder: DynamixelSDK-master\cpp\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).
{{ ::ii0.jpg?direct |}}

   *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
{{ ::ii1.jpg?direct |}}


   *Under Cpp > general > additional include directories include the path : DynamixelSDK-master\cpp\include ( depend on where you saved the github repository.
   *Under Linker > general > additional include directories include the path: DynamixelSDK-master\cpp\build\win64\output ( depend on where you saved the github repository.
   *Under Linker > input > additional dependencies add the library dxl_x64_cpp.lib
{{ ::ii2.jpg?direct |}}

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

{{ ::ii3.jpg?direct |}}
  
 Click in environment variables and under system variables find the variable path and click in edit
 
 Include the path :  DynamixelSDK-master\cpp\build\win64\output.

{{ ::ii4.jpg?direct |}}


----

===== FlyCapture Library installation guide =====

  * Download the [[https://www.ptgrey.com/flycapture-sdk|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 Cpp
  * 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 Cpp > 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
  * Go to environment variables and add two paths into the "path" variables under system variable : Program Files\Point Grey\bin64 and Program Files\Point Grey\bin64\vs2015


----
===== How to use Dynamixel SDK API's =====

 The control of the dynamixel servos in Cpp 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 model[[http://support.robotis.com/en/product/dynamixel/mx_series/mx-64.htm|mx-64 ]] this how the address table looks like:

{{ ::ii6.jpg?direct |}}

 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);
  fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK);

  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
}



</Code>

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");
	}
	else
	{
		printf("Failed to change the baudrate!\n");
		printf("Press any key to terminate...\n");
		getch();
		return 0;
	}

	
	// USB2Dynamixel successfully connected
	printf("Dynamixel has been successfully connected \n");
	printf("Press any Key to continue ");
	std::cout << "" << std::endl;

	cv::waitKey(10000000000);


</Code>

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

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

<Code>
int ADDR_MX_PRESENT_POSITION=36;

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

float position = dxl_present_position1;

</Code>

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);

</Code>

 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 [[http://support.robotis.com/en/product/dynamixel/mx_series/mx-64.htm|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

{{ ::ii9.jpg?direct |}} 


----

===== 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();
	if (error == PGRERROR_ISOCH_BANDWIDTH_EXCEEDED)
	{
		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;
	cv::waitKey(300000000);


</Code>

 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:

<Code>
                           // 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));

</Code>

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



----
===== Integrating vision and control =====

 The final part of this tutorial is to integrate the dynamixel control with the vision provided by the Flea3 camera. Is important that you have all the libraries working in the same Visual Studio Solution.
 There are many ways to do the Visual Servoing , and it's up to the programmer decide how to integrate both. 

 My first code to start to perform the Peg-in-Hole follows the pseudo-algorithm:

  - Apply circle detection function to detect hole
  - Find the biggest circle detected
  - Get radius and center point of the biggest circle detected
  - Calculate the distance from the hole to a desired setpoint
  - Based on the distance error , move the manipulator up/down/right/left
  - if the radius detected is bigger than a threshold the peg is in the hole and must release the gripper
  - if not cycle again.

The idea here is to make the UAV to hover over hole , and make it go down slowly. As the UAV is moving down next to the hole , the arm will keep the peg centered into the hole . When the radius detected is bigger than a threshold , the camera is very near to the hole and peg should be inside the hole this time , so the gripper must release the peg and the task is done.

 This is a very simple control algorithm and it will be used to do the preliminary tests with the MM-UAV. We need to see how many disturbance will be created by the arm movement , and how to program a more robust hover control to the UAV in order to make it hold a position next to the hole while moving down.

 In my code , the angles to make the moving movement are calculated in order to keep the end effector always in the vertical position , because if the end effector is a little bit offset from the vertical position , the peg will not enter in the hole properly , causing a big disturbance in the UAV and this can cause a crash.