User Tools

Site Tools


drexel_darwin_ball_tracking_webots

Differences

This shows you the differences between two versions of the page.

Link to this comparison view

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>​
drexel_darwin_ball_tracking_webots.txt ยท Last modified: 2016/11/06 20:41 by dwallace