VVV10 Turtle

From Wiki for iCub and Friends
Revision as of 18:29, 26 July 2010 by Paulfitz (talk | contribs)
(diff) ← Older revision | Latest revision (diff) | Newer revision → (diff)
Jump to: navigation, search

For reference, here's the code we wrote during this tutorial. I'll try to capture its evolution, more or less, reconstructing what happened during the tutorial.

Evolving the server

First, I wrote the world's world simulator of a turtle. We decided that we'd like to control the turtle with a forward speed and a rotational speed. I specified the units as "unspecified", and inflicted something like the following code on the world:

 #include <yarp/os/all.h>
 #include <yarp/sig/all.h>
 #include <yarp/dev/all.h>
 #include <math.h>
 using namespace yarp::os;
 using namespace yarp::sig;
 using namespace yarp::dev;
 int main(int argc, char *argv[]) {
   double x = 0, y = 0, theta = 0;
   double vfwd = 0, vturn = 0;
   double scale = 0.01;
   double scale2 = 0.01;
   while (true) {
     double dx = vfwd * cos(theta) * scale;
     double dy = vfwd * sin(theta) * scale;
     double dtheta = vturn * scale2;
     x += dx;
     y += dy;
     theta += dtheta;
     printf("at %g %g facing %g\n", x, y, theta);
     Time::delay(0.01);
   }
   return 0;
 }

Saved this as "main.cpp", compiled and ran this using these commands (on linux):

 yarp cmake
 mkdir build
 cd build
 cmake ..
 make
 ./yarpy

Saw printfs of a turtle going nowhere. Success!

Next step was to turn this into a YARP "device". That just means making it into a class with an "open" and "close" method. As follows:

 #include <yarp/os/all.h>
 #include <yarp/sig/all.h>
 #include <yarp/dev/all.h>
 #include <math.h>
 using namespace yarp::os;
 using namespace yarp::sig;
 using namespace yarp::dev;
 class Turtle : public DeviceDriver, public Thread {
   double x, y, theta;
   double vfwd, vturn;
   double max_speed;
 public:
   Turtle() {
     x = y = theta = 0;
     vfwd = vturn = 0;
     max_speed = 1;
   }
   virtual bool open(Searchable& option) {
     max_speed = option.check("max_speed",Value(1.0),"how fast can we go?").asDouble();
     start();
     return true;
   }
   virtual bool close() {
     stop();
     return true;
   }
   virtual void run() {
     double scale = 0.01;
     double scale2 = 0.01;
     while (!isStopping()) {
       double dx = vfwd * cos(theta) * scale;
       double dy = vfwd * sin(theta) * scale;
       double dtheta = vturn * scale2;
       x += dx;
       y += dy;
       theta += dtheta;
       printf("at %g %g facing %g  // max_speed %g\n",x, y, theta, max_speed);
       Time::delay(0.01);
     }
   }
 };
 int main(int argc, char *argv[]) {
   Property p;
   p.fromCommand(argc,argv);
   Turtle t;
   t.open(p);
   Time::delay(3);
   t.close();
   return 0;
 }

Added a "max_speed" option as an example. Used a yarp::os::Thread to run the simulation (a yarp::os::RateThread would have been a better choice, using Time::delay as I did won't give a reliable update rate). Now, when compiled and run, we see a turtle going nowhere for 3 seconds. More success!

Now, we make our device support the yarp::dev::IVelocityControl interface:

 #include <yarp/os/all.h>
 #include <yarp/sig/all.h>
 #include <yarp/dev/all.h>
 #include <math.h>
 using namespace yarp::os;
 using namespace yarp::sig;
 using namespace yarp::dev;
 
 class Turtle : public DeviceDriver, public Thread, public IVelocityControl {
 private:
   double x, y, theta;
   double vfwd, vturn;
   double max_speed;
 public:
   Turtle() {
     x = y = theta = 0;
     vfwd = vturn = 0;
     max_speed = 1;
   }
   virtual bool open(Searchable& option) {
     max_speed = option.check("max_speed",Value(1.0),"how fast can we go?").asDouble();
     start();
     return true;
   }
   virtual bool close() {
     stop();
     return true;
   }
   virtual void run() {
     double scale = 0.01;
     double scale2 = 0.01;
     while (!isStopping()) {
       double dx = vfwd * cos(theta) * scale;
       double dy = vfwd * sin(theta) * scale;
       double dtheta = vturn * scale2;
       x += dx;
       y += dy;
       theta += dtheta;
       printf("at %g %g facing %g  // max_speed %g\n", x, y, theta, max_speed);
       Time::delay(0.01);
     }
   }
   /**
    * Get the number of controlled axes. This command asks the number of controlled
    * axes for the current physical interface.
    * parame axes pointer to storage
    * @return true/false.
    */
   virtual bool getAxes(int *axes) {
     *axes = 2;
     return true;
   }
   /**
    * Set position mode. This command
    * is required by control boards implementing different
    * control methods (e.g. velocity/torque), in some cases
    * it can be left empty.
    * @return true/false on success failure
    */
   virtual bool setVelocityMode() {
     return true;
   }
   /**
    * Start motion at a given speed, single joint.
    * @param j joint number
    * @param sp speed value
    * @return bool/false upone success/failure
    */
   virtual bool velocityMove(int j, double sp) {
     if (j==0) {
       vfwd = sp;
     } else {
       vturn = sp;
     }
     return true;
   }
   /**
    * Start motion at a given speed, multiple joints.
    * @param sp pointer to the array containing the new speed values
    * @return true/false upon success/failure
    */
   virtual bool velocityMove(const double *sp) {
     vfwd = sp[0];
     vturn = sp[1];
     return true;
   }
   /** Set reference acceleration for a joint. This value is used during the
    * trajectory generation.
    * @param j joint number
    * @param acc acceleration value
    * @return true/false upon success/failure
    */
   virtual bool setRefAcceleration(int j, double acc) {
     return false;
   }
   /** Set reference acceleration on all joints. This is the valure that is
    * used during the generation of the trajectory.
    * @param accs pointer to the array of acceleration values
    * @return true/false upon success/failure
    */
   virtual bool setRefAccelerations(const double *accs) {
     return false;
   }
   /** Get reference acceleration for a joint. Returns the acceleration used to 
    * generate the trajectory profile.
    * @param j joint number
    * @param acc pointer to storage for the return value
    * @return true/false on success/failure
    */
   virtual bool getRefAcceleration(int j, double *acc) {
     return false;
   }
   /** Get reference acceleration of all joints. These are the values used during the
    * interpolation of the trajectory.
    * @param accs pointer to the array that will store the acceleration values.
    * @return true/false on success or failure 
    */
   virtual bool getRefAccelerations(double *accs) {
     return false;
   }
   /** Stop motion, single joint
    * @param j joint number
    * @return true/false on success or failure
    */
   virtual bool stop(int j) {
     velocityMove(j,0);
     return true;
   }
   /** Stop motion, multiple joints 
    * @return true/false on success or failure
    */
   virtual bool stop() {
     stop(0);
     stop(1);
     return true;
   }
 };
 
 int main(int argc, char *argv[]) {
   Network yarp;
   Drivers::factory().add(new DriverCreatorOf<Turtle>("turtle", "controlboard", "Turtle"));
   Property p;
   p.fromCommand(argc,argv);
   if (p.check("file")) {
     p.fromConfigFile(p.find("file").asString());
   }
   PolyDriver dev;
   if (!dev.open(p)) { printf("oops!\n"); }
   IVelocityControl *vel;
   dev.view(vel);
   if (vel==NULL) printf("oops! no vel\n");
   vel->velocityMove(0,0.1);
   Time::delay(10000);
   dev.close();
   return 0;
 }

In main, I also created the Turtle object in an indirect way, via a factory method. We can start the program like this:

 ./<executable-name> --device turtle --max_speed 3

or like this:

 ./<executable-name> --device controlboard --name /turtle --subdevice turtle --max_speed 3

(or we can make a configuration file and load it with the --file option). The second option creates a network interface for the turtle, of the same kind as used by iCub and other robots. Here's an example "client" for that interface:

 #include <yarp/os/all.h>
 #include <yarp/sig/all.h>
 #include <yarp/dev/all.h>
 using namespace yarp::os;
 using namespace yarp::sig;
 using namespace yarp::dev;
 int main(int argc, char *argv[]) {
   Network yarp;
   Property p;
   p.put("device","remote_controlboard");
   p.put("remote","/turtle");
   p.put("local","/me");
   PolyDriver dev;
   if (!dev.open(p)) { printf("oops!\n"); }
   IVelocityControl *vel;
   dev.view(vel);
   if (vel==NULL) printf("oops! no vel\n");
   vel->velocityMove(0,-100);
   return 0;
 }

The server and client programs can be compiled (in separate directories) using a CMakeLists.txt generated by running "yarp cmake" - or write your own (see http://eris.liralab.it/yarpdoc/d6/d66/yarp_cmake_hello.html).