Main Page | Modules | Class Hierarchy | Compound List | File List | Compound Members | File Members | Related Pages

Basic controller example: PushupController

This section describes a more complicated module: a RHex-specific controller which controls the robot to continuously do ``pushups''. It demonstrates the simplest way of building a controller in RHexLib, how basic motor control tasks are accomplished. It also shows some of the configuration facilities commonly used in RHexLib.

This controller assumes that RHex is in the standing position to start with. Starting there, it brings all the legs to a vertical position (making the robot sit), waits there for a moment, and then stands up again, restarting the cycle.

In this section, we present the PushupModule implementation in isolation. Unlike our previous example, a complete implementation of an example executable requires many other RHex specific components such as calibration tools which are not available in the core RHexLib. In any case, the source code associated with this tutorial can be found under the examples directory.

The Module Definition

Here we describe the details of how to define the PushupController module class and explain some of the components required by this controller. Usually, the definitions of module classes in RHexLib are done in separate header files, defining the interface of the module with the rest of the library. In this case, the class definition will reside in PushupController.hh.

#include "ModuleManager.hh"
#include "PositionControl.hh"
#include "Profiler.hh"

class PushupController : public Module {

public:
  PushupController ( void )
    : Module( "pushupcontroller", 0, true, false ) { };

  // Module base class interface
  void  init ( void );
  void  uninit ( void ) { };
  void  activate ( void );
  void  deactivate ( void );
  void  update ( void );

This part of the class definition is straightforward. We simply define the standard module functions in a way similar to the previous example, the PrintModule. Once again, the name and index of the module are determined in the definition of the class constructor, through the instatiation of the base class Module. Note also that in this case, all the module functions except the uninit() method will be defined in the implementation file PushupController.cc.

Next we define a few private variables nad methods, local to PushupController.

private:

  PositionControl *control[6];

These member variables will be pointers to the six different position controller modules, associated with each one of the six legs. PositionControl modules implement basic proportional derivative(PD) control feedback for all the motors in the system. Through their public interface, our controller will be able to set the target positions for each leg, updated every millisecond, commanding the legs perform the desired actions.

  MotorGains_t     oldGains[6];

These member variables hold the PD gains that may have been in use by the position controllers before the pushup controller is activated. When the controller is deactivated, it will restore these gains to leave the position controllers in their previous state. This is usually a good practice because other components in the system might be expecting the PD gains to remain unchanged for proper operation.

  Profiler         legProfiler;

This member variable instantiates a Profiler object (see Profiler.hh in the examples directory), which is responsible from generating the leg position as a function of time, also called leg profile. Note that we only have one such object because, for the pushup controller, all the legs are synchronized. Cconsequently, the desired positions for all six legs will be the same at all times.

In RHexLib, the actual PD control of the leg motors and the generation of the associated position profile are performed separately. Every millisecond, our controller will ask the leg profiler the desired position for all the legs. Then, it will set this new target on all six position controllers. This scheme abstracts the generation of the desired leg trajectories from the local PD feedback loop on the leg motors.

  void trackCurrentProfile( void );

This utility method provides a facility to read the current value of the leg profiler and inform all the motor position controllers about the new target.

  double lowerTime;  // Time in seconds to lower the robot
  double chillTime;  // Time in seconds to chill, sitting down
  double standTime;  // Time in seconds to raise the robot up
  double waitTime;   // Time in seconds to wait standing up.

These member variables determine how much time the controller should spend in each of its stages. The pushup behavior can be decomposed into four stages: lowering, chilling after sitting down, standing up and waiting in a standing position. These variables determine how much time is spent in each of these stages.

  int phase;

This member variable indicates the current discrete ``phase'' of the controllers. In implementing the aforementioned four different stages, the pushup controller goes through six phases, where the controller actions are defined as follows:

Another way to write this controller is to use the StateMachine facilites of RHexLib. For this relatively simple example, we do not need the power of state machines. If there were many more than six states, or if the states were not visited in a simple cycle, or if the events that trigger state transitions are were not so simple, state machines would probably make the task simpler. State Machines are covered in later tutorials about a toycontroller" and a supervisory controller.

  double mark;
};

This member variable is used to hold the state time of each phase in order to switch to the next phase at appropriate times, determined by the values of the timing member variables described above.

The init Method

There are two tasks usually performed by the init method of modules: 1) locate other modules that are going to be used; and 2) initialize member variables by reading configuration information from a resource file..

With the function MMFindModule(), RHexLib provides a means to acquire a pointer to other module objects in the system. Almost all the functionality of the library is implemented with modules, including low level motor control, higher level behaviors such as walking, standing, sitting, pronking as well as utilities such as logging data. Normally, in main(), all such modules to be used by an executable program are ``added'' to the system through calls to MMAddModule(). The module manager thus has a list of all these components, and can find modules by their name and index, which are two of the parameters to the MMFindModule() function.

void PushupController::init( void ) {
  int i;

  for ( i = 0; i < 6; i ++ ) {
    if ( ( control[i] = ( PositionControl * ) 
           MMFindModule( POSITIONCONTROL_NAME, i )) == NULL)
      MMFatalError ( "PushupController::init", 
                     "Cannot find motor controller!" );
  }

This part of the init method locates all six PositionControl modules. All of these six modules have the same name, defined in StdModules.hh as the macro POSITIONCONTROL_NAME (defined in PositionControl.hh), and their index identifies which motor axis they correspond to. If MMFindModule() function call returns NULL, indicating that the required module has not yet been added to the module manager's list, a fatal error is generated, and the program exits.

  lowerTime = MMGetFloatSymbol( "pushup_lower_time" , 1.0 );
  chillTime = MMGetFloatSymbol( "pushup_chill_time" , 0.3 );
  standTime = MMGetFloatSymbol( "pushup_stand_time" , 0.4 );
  waitTime = MMGetFloatSymbol( "pushup_wait_time" , 0.3 );
}

The second part of the init method initializes the timing configuration of the pushup machine. RHexLib maintains a global symbol table, where symbol names are associated with either a floating point or a string value. Modules in the system can access the table using MMGetFloatSymbol() and MMGetStringSymbol() to retrieve the value of a particular symbol. These functions also take a default value as an argument, to be returned if the requested symbol cannot be found.

Usually, the symbol table is initialized by loading a ``configuration file'' using the MMReadConfig() function in main(). This configuration file includes statements such as:

  pushup_lower_time = 0.9;
  pushup_chill_time = 0.2;
  pushup_stand_time = 0.5;
  pushup_wait_time = 0.2;

which assign values to symbols. This mechanism provides means of reconfiguring the behavior of controllers and modules without requiring that the executable be recompiled.

The activate Method

The only modules used by the pushup controller are the motor position controllers. Moreover, they are required throughout the whole operation of the controller. As a consequence, the activate module method in this case is responsible for requesting exclusive access to all six position controllers and adjusting the PD gains appropriately.

void PushupController::activate( void ) {
  int          i;
  MotorGains_t gains;

  phase = 0;

Every time the pushup controller is activated, it will start from phase 0: starting to lower. The phase must be initialized in the activate method because the same instantiation of this module may be activated more than once (at different times) by different supervisory controllers.

  for ( i = 0; i < 6; i++ )
    MMGrabModule( control[i], this );

This statement requests exclusive access to the motor position controller modules whose pointers were acquired in the init method. The functions MMGrabModule() and MMReleaseModule() are thus used in pairs to acquire and release the exclusive use of other modules in the system. They should be used whenever the functionality of a module is needed to make sure that the operation of other components in the system which might be using the same modules is not disturbed.

In this context, there are two types of modules: SINGLE_USER and MULTI_USER. The type of a module is determined by the constructor of the Module base class. The behavior of MMGrabModule() is different for these two types.

Single user modules can be grabbed by only one other module at a time. If a second grab request is received, the system gives a fatal error and exits. This mechanism makes sure that, for example, the motor position controllers are not used by more than one entity at a time, which could result in pretty chaotic behavior. Multi user modules can be grabbed as many times as necessary.

Another feature of MMGrabModule() is that the grabbed module is also activated if it is not already active. Similarly, the MMReleaseModule() function deactivates the module if it has been released as many times as it has been grabbed. Hence, for multi user modules, the first call to the grab function activates the module and the last call to the release function deactivates the module.

  gains.kp = 5.0;
  gains.kd = 0.14;

  for ( i = 0; i < 6; i++ ) {
    control[i]->getGains( & oldGains[i] );
    control[i]->setGains( & gains );
  }
}

This code segment saves the PD gains of the motor controllers and sets the new gains to appropriate values for the pushup behavior.

deactivate Method

The deactivate module method is usually responsible for releasing the modules that have been grabbed in the activate method. In addition, it usually restores their states to what they were before they were grabbed.

void PushupController::deactivate( void ) {
  int i;

  for ( i = 0; i < 6; i++ )
    control[i]->setGains( & oldGains[i] );

This code segment restores the PD gains of the motor position controllers to their old values.

  for ( i = 0; i < 6; i++ )
    MMReleaseModule( control[i], this );
}

Finally, all six motor position controllers are released to grant access to anybody else who wants to use them.

The update Method

The update module method is where the bulk of the work is done for the controller. All the other module methods are mainly for initialization and clean integration with the rest of the system and do not much else.

Depending on the current phase of the controller, the update method, being called once every millisecond, has to perform different tasks. As a consequence, it ends up being a big case statement, implementing a primitive state machine which linearly proceeds through 6 states (or phases). In later sections, we will see more structured and formal ways of defining state machines, which will allow the programmer to define much more complex behaviors. Beyond 5 or 6 states, with transitions depending on events other than simple timers, defining state machines through case statements quickly becomes unelegant.

void PushupController::update( void ) {

  int           i;
  MotorTarget_t target;
  double        now = MMReadTime();

This segment defines the local variables. An important function to note is MMReadTime, which returns the current time in seconds.

  switch ( phase ) {
  case 0:   // Starting to lower the robot

    legProfiler.setup( now, now + lowerTime, 0, M_PI / 2 );

As described in the preceding sections, phase 0 in the pushup controller is when the robot starts to lower. Hence, in this state, we initialize the leg profile to bring the leg to a horizontal position and immediately proceed with the next phase.

The setup(t1, t2, val2, val2) method of the Profiler class is used to initialize a linear time trajectory for the leg between two angle values. The resulting trajectory has the following functional form:

$ f(t) = \left\{\begin{array}{l@{\hspace{0.3in}}l} val_1 & t < t_1 \\ val_1 + {(t-t_1) (val_2-val_1)}/{t_2-t_1)} & t_1 \leq t \leq t_2 \\ val_2 & t > t_2 \\ \end{array}\right. $

Note that this interface does not know anything about the $2 \pi$ periodicity of angles, and interprets $val_1$ and $val_2$ as real numbers. The pushup controller, in order to lower the robot to the ground, sets up a linear profile starting from 0 (vertical legs) and going to $\pi/2$ (horizontal legs) in the amount of time configured in the init method.

    mark = now;
    phase = 1;
    break;

This phase of execution also marks the current time to measure how much more the controller needs to wait before initiating the chilling phase. Also, the assignment to the phase member variable transitions into phase 1, which waits for the robot lowering to end.

  case 1:   // Waiting for the robot to lower

    if ( now < ( mark + lowerTime ) )
      trackCurrentProfile();
    else {
      mark = now;
      phase = 2;
    }
    break;

During phase 1, the controller does not change the profile which has been setup in phase 0. However, it reads the current target position from the profile and commands the motors to go to the current target angle through repeated calls to the trackCurrentProfile method (whose details are explained later). When the legs reach their horizontal position (i.e. when lowerTime amount of time has elapsed), the controller switches to phase 2.

  case 2:   // Wait for a while in the sitting position

    if ( now > ( mark + chillTime ) ) {
      mark = now;
      phase = 3;
    }
    break;

During this phase, the controller simply waits for a configured amount of time, without commanding new targets for the legs. Due to the fact that the position control modules retain their latest assigned target, the legs maintain their horizontal positions, chilling in a sitting down position. Once again, note the use of the mark member variable, which is a typical way of handling time-based transitions between different controller phases.

  case 3:   // Starting to standup

    legProfiler.setup( now, now + standTime, M_PI / 2, 0 );
    mark = now;
    phase = 4;

    break;

Similar to phase 0, our controller next sets up a new leg trajectory in this phase. This time, however, the legs are commanded to go from $\pi / 2$ to $0$, effectively raising the robot to a standing position.

  case 4:   // Waiting for the robot to standup
    if ( now < ( mark + standTime ) )
      trackCurrentProfile();
    else {
      mark = now;
      phase = 5;
    }

    break;

  case 5:   // Wait for a while in the standing position
    if ( now > ( mark + waitTime ) ) {
      mark = now;
      phase = 0;
    }
    break;

  }
}

Phases 4 and 5 perform exactly the same functionality as the phases 1 and 2, first tracking the leg profiles for standing up, and then waiting a little while in a standing position. Note that the controller returns to phase 0 once done with phase 5, restarting the sequence by lowering the robot to the ground.

The trackCurrentProfile Method

As explained in the preceding sections, the commonly performed task of reading the desired positions for the legs from the leg profiler and setting the targets of the position controllers is done by this method.

void PushupController::trackCurrentProfile ( void ) {

  int i;
  MotorTarget_t target;

  target = legProfiler.getVal();

  // Command all 6 motor controllers to go to that position.
  for ( i = 0; i < 6; i++ )
    control[i]->setTarget ( & target );
}

The data type MotorTarget_t holds the desired position, velocity and acceleration for the motor, which is used by the local PD feedback control. The leg profiler extracts this information from the functional form given in the previous section, and returns it through its getVal() method. This target can then be directly passed into PositionControl::setTarget().

RHexLib Reference Documentation