Motor control

From Wiki for iCub and Friends
Jump to: navigation, search

Motor control in Yarp, a brief tutorial

Motor control in Yarp is done through a device driver. For more documentation please see: yarp2 documentation and the related classes. The project on the iCub repository under src/simple_headcontrol is a good example to start playing with motor control. This example uses a remoted device driver which exports exactly the same interfaces of the actual device driver (initialization is different). Clearly, the main difference between the two is in terms of efficiency, if you're developing a low-level control loop you might want to be local, on the other hand a simple sporadic position control can be effected through the remote device driver. Let's assume we have started the server side already (e.g. using yarpdev).

In practice, we start by preparing a set of configuration parameters:

 Property options;
 options.put("robot", "icub"); // typically from the command line.
 options.put("device", "remote_controlboard");
 Value& robotname = options.find("robot");
 yarp::String s("/");
 s += robotname.asString();
 s += "/head/control";
 options.put("local", s.c_str());
 s.clear();
 s += "/";
 s += robotname.asString();
 s += "/head";
 options.put("remote", s.c_str());

where the local and remote parameters are used to set the port names for the connection to the server side. In our example this means that the process will create the following (local) ports:

  /icub/head/control/state:i
  /icub/head/control/command:o
  /icub/head/control/rpc:o

And will automatically connect them to the server, on the following (remote) ports (we assume here that the server created those ports correctly):

  /icub/head/state:o
  /icub/head/command:i
  /icub/head/rpc:i


To create the driver we use the PolyDriver:

 PolyDriver dd(options);
 if (!dd.isValid()) {
     ACE_OS::printf("Device not available.  Here are the known devices:\n");
     ACE_OS::printf("%s", Drivers::factory().toString().c_str());
     Network::fini();
     return 1;
 }

which instantiates the driver. If the parameters are correct and the server side has been prepared the local driver also connects to the remote one. The next step is to get a set of interfaces (pointers to) to work with, for example:

   IPositionControl *pos;
   IVelocityControl *vel;
   IEncoders *enc;
   IPidControl *pid;
   IAmplifierControl *amp;
   IControlLimits *lim;

and:

   dd.view(pos);
   dd.view(vel);
   dd.view(enc);
   dd.view(pid);
   dd.view(amp);
   dd.view(lim);

if everything went ok the pointers are valid pointers (check it!). For example:

   if (pos==0) {
     ACE_OS::printf("Error getting IPositionControl interface.\n");
     Network::fini();
     return 1;
   } 
   ...

We can thus start with checking how many axes we can control by doing:

   int jnts = 0;
   pos->getAxes(&jnts);

Let's now initialize the controllers and start up the amplifiers:

   // we need to set reference accelerations used to generate the velocity
   // profile, here 50 degrees/sec^2
   int i;
   for (i = 0; i < jnts; i++) {
       tmp[i] = 50.0;
   }
   pos->setRefAccelerations(tmp);
   // enable the amplifier and the pid controller on each joint
   for (i = 0; i < jnts; i++) {
       amp->enableAmp(i);
       pid->enablePid(i);
   }

if needed we can check the position of our axes by doing:

 enc->getEncoders(q);

which reads the values of the motor encoders into a vector of doubles of size jnts.

  • Velocity control. Issue a:
 bool ok = vel->velocityMove(control_velocity);

which for example accelerates all axes to the velocity described by the vector control_velocity (in degrees per second).

  • Position control. First do:
 int i;
 for (i = 0; i < jnts; i++) {
     tmp[i] = 40.0;
 }
 pos->setRefSpeeds(tmp);

which sets the reference speed for all axes to 40 degrees per second. The reference speed is used to interpolate the trajectory between the current and the desired position.

Now you can do:

 bool ok = pos->positionMove(control_position);

where control_position is the desired position. This starts a movement of all axes toward the desired position. The position and velocity commands can be intermixed (presently we have a bug though which make the controller to get stuck, sorry!).

  • Closing the robot. When done with the robot, the application can finalize the control sequence by disabling the amplifiers and closing the device driver.
 /* warning: this for() loop kills the motors: */
 //for (i = 0; i < jnts; i++) {
 //    amp->disableAmp(i);
 //    pid->disablePid(i);
 //}
 /* this just closes the driver: OK */
 dd.close();

Notes on the local device

The EsdMotionControl class in libYARP_dev is an example of a local device driver. The procedure to instantiate it is conceptually identical to the sequence illustrated here apart from the parameters passed to open() which are more numerous and complicated. These are typically stored in a .ini file and loaded through the Property class and the fromConfigFile() method.

An alternate client

Another client is available in the iCub repository at src/simple_client.

Efficiency issues

Encoder values and commands are read or written through streaming ports. All other calls are RPC with return values and thus potentially slower. Ports on the server side are instantiated accordingly. They are:

  • state port: streams the state of the robot, that is the encoder angles every few milliseconds. The data type is a Vector of double of length equal to the number of controlled axes.
  • command port: receives the command, it accepts a Bottle with a single Vocab item (either VOCAB4('p','o','s','s') or VOCAB4('v','m','o','s')) specifying a position or velocity command respectively. The Bottle is followed by a Vector of double of length equal to the number of controlled axes.
  • RPC port: accepts Bottles in various formats according to the names defined in ControlBoardInterfaces.h. The Bottles are made as follows:
    • VOCAB_SET followed by the Vocab specifying the command and the parameters (which are a function of the specific command). The return value is either a VOCAB_OK or a VOCAB_FAILED.
    • VOCAB_GET followed by the message type (another Vocab) and the parameters of the request. The return Bottle is formed by VOCAB_IS followed by the command name, the actual response (variable format) and finally by the VOCAB_OK or VOCAB_FAILED. If the call fails before producing an output the VOCAB_FAILED is sent alone as the only element of the reply message.