robotic_manipulators_ibvs
Differences
This shows you the differences between two versions of the page.
Both sides previous revisionPrevious revisionNext revision | Previous revision | ||
robotic_manipulators_ibvs [2016/08/08 16:54] – joaomatos | robotic_manipulators_ibvs [2016/10/23 20:16] (current) – dwallace | ||
---|---|---|---|
Line 1: | Line 1: | ||
===== Image Based Visual Servoing ===== | ===== Image Based Visual Servoing ===== | ||
- | This tutorial will show how to configure and program the Dynamixels servos in C++ using 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. | + | **Author:** Joao Matos Email: < |
+ | \\ | ||
+ | **Date:** Last modified on 6/8/2016 | ||
+ | \\ | ||
+ | **Keywords: | ||
+ | \\ | ||
+ | |||
+ | 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. | ||
| | ||
Line 24: | Line 31: | ||
{{ :: | {{ :: | ||
+ | |||
+ | 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. | ||
---- | ---- | ||
Line 30: | Line 39: | ||
| | ||
- | *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). | + | *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). |
{{ :: | {{ :: | ||
Line 38: | Line 47: | ||
- | | + | |
- | | + | |
| | ||
{{ :: | {{ :: | ||
Line 49: | Line 58: | ||
Click in environment variables and under system variables find the variable path and click in edit | Click in environment variables and under system variables find the variable path and click in edit | ||
- | | + | |
{{ :: | {{ :: | ||
Line 59: | Line 68: | ||
* Download the [[https:// | * Download the [[https:// | ||
- | * 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++ | + | * 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 included. | + | * Open a solution in the Visual Studio where you already have OpenCV |
* Right click the solution in the solution explorer and go to properties. | * 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 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 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 | * Under Link > input > additional dependencies include the library: FlyCapture2d_v140.lib | ||
+ | * Go to environment variables and add two paths into the " | ||
Line 70: | Line 80: | ||
===== How to use Dynamixel SDK API's ===== | ===== 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. | + | 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. |
| | ||
Line 80: | Line 90: | ||
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 . | 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: | ||
+ | |||
+ | < | ||
+ | #ifdef __linux__ | ||
+ | #include < | ||
+ | #include < | ||
+ | #include < | ||
+ | #elif defined(_WIN32) || defined(_WIN64) | ||
+ | #include < | ||
+ | #endif | ||
+ | |||
+ | #include < | ||
+ | #include " | ||
+ | |||
+ | using namespace FlyCapture2; | ||
+ | |||
+ | // Protocol version | ||
+ | #define PROTOCOL_VERSION | ||
+ | |||
+ | // Default setting | ||
+ | | ||
+ | #define BAUDRATE | ||
+ | #define DEVICENAME | ||
+ | |||
+ | int getch() | ||
+ | { | ||
+ | #ifdef __linux__ | ||
+ | struct termios oldt, newt; | ||
+ | int ch; | ||
+ | tcgetattr(STDIN_FILENO, | ||
+ | newt = oldt; | ||
+ | newt.c_lflag &= ~(ICANON | ECHO); | ||
+ | tcsetattr(STDIN_FILENO, | ||
+ | ch = getchar(); | ||
+ | tcsetattr(STDIN_FILENO, | ||
+ | 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, | ||
+ | newt = oldt; | ||
+ | newt.c_lflag &= ~(ICANON | ECHO); | ||
+ | tcsetattr(STDIN_FILENO, | ||
+ | oldf = fcntl(STDIN_FILENO, | ||
+ | fcntl(STDIN_FILENO, | ||
+ | |||
+ | ch = getchar(); | ||
+ | |||
+ | tcsetattr(STDIN_FILENO, | ||
+ | fcntl(STDIN_FILENO, | ||
+ | |||
+ | 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. | ||
+ | |||
+ | < | ||
+ | // Initialize PortHandler instance | ||
+ | // Set the port path | ||
+ | // Get methods and members of PortHandlerLinux or PortHandlerWindows | ||
+ | dynamixel:: | ||
+ | |||
+ | // Initialize PacketHandler instance | ||
+ | // Set the protocol version | ||
+ | // Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler | ||
+ | dynamixel:: | ||
+ | |||
+ | |||
+ | int dxl_comm_result = COMM_TX_FAIL; | ||
+ | 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-> | ||
+ | { | ||
+ | printf(" | ||
+ | } | ||
+ | else | ||
+ | { | ||
+ | printf(" | ||
+ | printf(" | ||
+ | getch(); | ||
+ | return 0; | ||
+ | } | ||
+ | |||
+ | // Set port baudrate to 1000000 | ||
+ | //All the dynamixels must be using baudrate 1000000 , configure this on the Dynamixel Wizard | ||
+ | if (portHandler-> | ||
+ | { | ||
+ | printf(" | ||
+ | } | ||
+ | else | ||
+ | { | ||
+ | printf(" | ||
+ | printf(" | ||
+ | getch(); | ||
+ | return 0; | ||
+ | } | ||
+ | |||
+ | |||
+ | // USB2Dynamixel successfully connected | ||
+ | printf(" | ||
+ | printf(" | ||
+ | std::cout << "" | ||
+ | |||
+ | cv:: | ||
+ | |||
+ | |||
+ | </ | ||
+ | |||
+ | This will open and configure the port to communicate with the USB2Dynamixel . | ||
+ | |||
+ | ** To read something (get information) from any dynamixel servo the API is:** | ||
+ | |||
+ | < | ||
+ | int ADDR_MX_PRESENT_POSITION=36; | ||
+ | |||
+ | dxl_comm_result = packetHandler-> | ||
+ | |||
+ | 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 " | ||
+ | |||
+ | ** If we want to write something (send information) the idea is very similar to the reading:** | ||
+ | |||
+ | < | ||
+ | int ADDR_MX_GOAL_POSITION=30; | ||
+ | int gripper=560; | ||
+ | |||
+ | dxl_comm_result = packetHandler-> | ||
+ | |||
+ | </ | ||
+ | |||
+ | What is changed here is after the arrow from the packetHandler -> we use " | ||
+ | |||
+ | **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:// | ||
+ | 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: | ||
+ | |||
+ | < | ||
+ | //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 << " | ||
+ | return false; | ||
+ | } | ||
+ | |||
+ | // Get the camera info and print it out | ||
+ | error = camera.GetCameraInfo(& | ||
+ | if (error != PGRERROR_OK) | ||
+ | { | ||
+ | std::cout << " | ||
+ | return false; | ||
+ | } | ||
+ | std::cout << camInfo.vendorName << " " | ||
+ | << camInfo.modelName << " " | ||
+ | << camInfo.serialNumber << std::endl; | ||
+ | |||
+ | error = camera.StartCapture(); | ||
+ | if (error == PGRERROR_ISOCH_BANDWIDTH_EXCEEDED) | ||
+ | { | ||
+ | std::cout << " | ||
+ | return false; | ||
+ | } | ||
+ | else if (error != PGRERROR_OK) | ||
+ | { | ||
+ | std::cout << " | ||
+ | return false; | ||
+ | } | ||
+ | |||
+ | |||
+ | std::cout << " Flea3 Camera Connected " << std::endl; | ||
+ | std::wcout << "Press any key to continue " << std::endl; | ||
+ | cv:: | ||
+ | |||
+ | |||
+ | </ | ||
+ | |||
+ | 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(& | ||
+ | |||
+ | // convert to rgb | ||
+ | Image rgbImage; | ||
+ | rawImage.Convert(FlyCapture2:: | ||
+ | |||
+ | // convert to OpenCV Mat | ||
+ | unsigned int rowBytes = (double)rgbImage.GetReceivedDataSize() / (double)rgbImage.GetRows(); | ||
+ | cv::Mat coloredimage = cv:: | ||
+ | |||
+ | //Resize the image to 640 x 480 | ||
+ | cv:: | ||
+ | |||
+ | </ | ||
+ | |||
+ | 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/ | ||
+ | - 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. |
robotic_manipulators_ibvs.1470700446.txt.gz · Last modified: 2016/08/08 16:54 by joaomatos