# VVV10 Turtle

(diff) ← Older revision | Latest revision (diff) | Newer revision → (diff)

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;
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).