Differences

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

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

DASL Wiki

User Tools

Site Tools

Differences

Page Tools