====== 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>