This shows you the differences between two versions of the page.
— |
drexel_darwin_ball_tracking_webots [2016/11/06 20:41] (current) dwallace created |
||
---|---|---|---|
Line 1: | Line 1: | ||
+ | ====== Darwin-OP Ball Tracking Simulation on Webots ====== | ||
+ | Simulation of the Darwin-OP on Webots is implemented using the Darwin-OP API/Managers. The Darwin-OP Gait Manager and the Vision Manager was used for this. | ||
+ | |||
+ | The Gait manager has a pre-defined walking algorithm for the Darwin-OP. The Gait manager allows us to make the Darwin-OP walk forward/backward and to make him turn. | ||
+ | |||
+ | The Vision manager is used to determine the center of the ball in the image frame. Which is required to determine how much the Darwin-OP has to walk and which direction to turn. | ||
+ | |||
+ | (These low level concepts have also been written from scratch on Gazebo.But since these pre-defined libraries were already written out for the Darwin-OP I decided to use them to implement the concept on Webots.) | ||
+ | |||
+ | Here is a video of the Darwin-OP tracking the ball and walking towards it. | ||
+ | |||
+ | {{youtube>jF78OW-L_hA?large}}\\ | ||
+ | |||
+ | <code c++> | ||
+ | #include "VisualTracking.hpp" | ||
+ | #include <webots/Servo.hpp> | ||
+ | #include <webots/LED.hpp> | ||
+ | #include <webots/Camera.hpp> | ||
+ | #include <Image.h> | ||
+ | #include <DARwInOPVisionManager.hpp> | ||
+ | #include <DARwInOPMotionManager.hpp> | ||
+ | #include <DARwInOPGaitManager.hpp> | ||
+ | #include <cstdlib> | ||
+ | #include <cmath> | ||
+ | #include <iostream> | ||
+ | #include <fstream> | ||
+ | |||
+ | using namespace webots; | ||
+ | using namespace managers; | ||
+ | using namespace std; | ||
+ | |||
+ | static const char *servoNames[NSERVOS] = { | ||
+ | "ShoulderR" /*ID1 */, "ShoulderL" /*ID2 */, "ArmUpperR" /*ID3 */, "ArmUpperL" /*ID4 */, | ||
+ | "ArmLowerR" /*ID5 */, "ArmLowerL" /*ID6 */, "PelvYR" /*ID7 */, "PelvYL" /*ID8 */, | ||
+ | "PelvR" /*ID9 */, "PelvL" /*ID10*/, "LegUpperR" /*ID11*/, "LegUpperL" /*ID12*/, | ||
+ | "LegLowerR" /*ID13*/, "LegLowerL" /*ID14*/, "AnkleR" /*ID15*/, "AnkleL" /*ID16*/, | ||
+ | "FootR" /*ID17*/, "FootL" /*ID18*/, "Neck" /*ID19*/, "Head" /*ID20*/ | ||
+ | }; | ||
+ | |||
+ | VisualBallTracking::VisualBallTracking(): | ||
+ | Robot() | ||
+ | { | ||
+ | mTimeStep = getBasicTimeStep(); | ||
+ | //mTimeStep =1; | ||
+ | mEyeLED = getLED("EyeLed"); | ||
+ | mHeadLED = getLED("HeadLed"); | ||
+ | mCamera = getCamera("Camera"); | ||
+ | mCamera->enable(2*mTimeStep); | ||
+ | | ||
+ | for (int i=0; i<NSERVOS; i++) | ||
+ | mServos[i] = getServo(servoNames[i]); | ||
+ | |||
+ | mMotionManager = new DARwInOPMotionManager(this); | ||
+ | mGaitManager = new DARwInOPGaitManager(this, "config.ini"); | ||
+ | mVisionManager = new DARwInOPVisionManager(mCamera->getWidth(), mCamera->getHeight(), 355, 15, 60, 15, 0.1, 30); | ||
+ | } | ||
+ | |||
+ | VisualBallTracking::~VisualBallTracking() { | ||
+ | } | ||
+ | |||
+ | void VisualBallTracking::myStep() { | ||
+ | int ret = step(mTimeStep); | ||
+ | if (ret == -1) | ||
+ | exit(EXIT_SUCCESS); | ||
+ | } | ||
+ | |||
+ | // function containing the main feedback loop | ||
+ | void VisualBallTracking::run() { | ||
+ | float xp[3] = {0,1,2}; | ||
+ | float footl_loc [] = {-1.22,0,-0.52886}; | ||
+ | float footr_loc [] = {1.22,0,0.52886}; | ||
+ | // float anklel_loc [] = {0,0,0}; | ||
+ | // float ankler_loc [] = {0,0,0}; | ||
+ | float kneel_loc [] = {-2.25,0,-1.0577}; | ||
+ | float kneer_loc [] = {2.25,0,1.0577}; | ||
+ | float hipPl_loc [] = {1.15,0,0.52886}; | ||
+ | float hipPr_loc [] = {-1.15,0,-0.52886}; | ||
+ | // float hipRl_loc [] = {0,0,0}; | ||
+ | // float hipRr_loc [] = {0,0,0}; | ||
+ | | ||
+ | float x = 0; | ||
+ | // float anklel; | ||
+ | // float ankler; | ||
+ | float footl; | ||
+ | float footr; | ||
+ | float kneel; | ||
+ | float kneer; | ||
+ | float hipPl; | ||
+ | float hipPr; | ||
+ | // float hipRl; | ||
+ | // float hipRr; | ||
+ | | ||
+ | while (x<=2) | ||
+ | { | ||
+ | footl = lip(x,xp,footl_loc); | ||
+ | footr = lip(x,xp,footr_loc); | ||
+ | kneel = lip(x,xp,kneel_loc); | ||
+ | kneer = lip(x,xp,kneer_loc); | ||
+ | hipPl = lip(x,xp,hipPl_loc); | ||
+ | hipPr = lip(x,xp,hipPr_loc); | ||
+ | |||
+ | mServos[15]->setPosition(footl ); | ||
+ | mServos[14]->setPosition(footr); | ||
+ | mServos[13]->setPosition(kneel); | ||
+ | mServos[12]->setPosition(kneer); | ||
+ | mServos[11]->setPosition(hipPl); | ||
+ | mServos[10]->setPosition(hipPr); | ||
+ | myStep(); | ||
+ | x = x+ 0.01; | ||
+ | |||
+ | } | ||
+ | |||
+ | myStep(); | ||
+ | |||
+ | while (true) { | ||
+ | double x, y; | ||
+ | //int z; | ||
+ | const unsigned char * image = mCamera->getImage(); | ||
+ | bool ballInFieldOfView = mVisionManager->getBallCenter(x, y,image); | ||
+ | //unsigned int* img = (unsigned int*) mCamera->getImage(); | ||
+ | // Eye led indicate if ball has been found | ||
+ | if(ballInFieldOfView) | ||
+ | mEyeLED->set(0x00FF00); | ||
+ | else | ||
+ | mEyeLED->set(0xFF0000); | ||
+ | std::cout << "mCamera->getWidth()/2 :" << mCamera->getWidth()/2 << endl; | ||
+ | std::cout << "x :" << x << endl; | ||
+ | std::cout << "y :" << y << endl; | ||
+ | // calculating the angle to turn. | ||
+ | float arc = atan((x - (mCamera->getWidth()/2))/(mCamera->getHeight()-y) ); | ||
+ | std::cout << "arc :" << arc << endl; | ||
+ | // Move the head in direction of the ball if it has been found | ||
+ | mGaitManager->start(); | ||
+ | if (y < 190) // ball far away, go quickly | ||
+ | {mGaitManager->setXAmplitude(1.0); | ||
+ | mGaitManager->setAAmplitude(-arc); | ||
+ | mGaitManager->step(mTimeStep);} | ||
+ | else | ||
+ | { | ||
+ | mGaitManager->stop(); | ||
+ | } | ||
+ | // step | ||
+ | myStep(); | ||
+ | } | ||
+ | } | ||
+ | |||
+ | float VisualBallTracking::lip(float x,float xp[],float yp[]) | ||
+ | { | ||
+ | float y=0; | ||
+ | float xp0=0; | ||
+ | float yp0=0; | ||
+ | float xp1=0; | ||
+ | float yp1=0; | ||
+ | for (int i =0; i<=2; i++) | ||
+ | { | ||
+ | if (xp[i]<=x) | ||
+ | { | ||
+ | xp0 = xp[i]; | ||
+ | yp0 = yp[i]; | ||
+ | } | ||
+ | if (xp[i]>x) | ||
+ | { | ||
+ | xp1 = xp[i]; | ||
+ | yp1 = yp[i]; | ||
+ | break; | ||
+ | } | ||
+ | } | ||
+ | float m = (yp1-yp0)/(xp1-xp0); | ||
+ | y = (yp0 +(m*(x-xp0))); | ||
+ | return (y); | ||
+ | } | ||
+ | |||
+ | </code> |