Exploring Tekkotsu Programming on Mobile Robots:

Motion Commands

Prev: Events
Up: Contents
Next: Postures and Motion Sequences

Contents: Motion commands, Setting the LEDs, Detecting completion, Moving the head, Effector names

Motion Commands and the Control of Effectors

Real-time systems are slaves to the clock. They achieve the illusion of smooth behavior by rapidly updating a set of control signals many times per second. For example, to smoothly turn a robot's head to the right, the head must accelerate, travel at constant velocity for a while, and then decelerate. This is accomplished by making many small adjustments to the motor torques. Another example: to get the robot's LEDs to blink repeatedly, they must be turned on for a certain period of time, then turned off for another length of time, and so forth. To get them to glow steadily at medium intensity, they must be turned on and off very rapidly.

The robot's operating system updates the states of all the effectors (servos, motors, LEDs, etc.) every few milliseconds. Each update is called a "frame", and can accommodate simultaneous changes to any number of effectors. On the AIBO, updates occur every 8 milliseconds and frames are buffered four at a time, so the application must have a new buffer available every 32 milliseconds; other robots may use different update intervals. In Tekkotsu these buffers of frames are produced by the MotionManager, whose job is to execute a collection of simultaneously active MotionCommands (MCs) of various types every few milliseconds. The results of these MotionCommands are assembled into a buffer that is passed to the operating system (Aperios for the AIBO, or Linux for other robots).

Suppose we want the robot to blink its LEDs on and off at a rate of once per second. What we need is a MotionCommand that will calculate new states for the LEDs each time the MotionManager asks for an update. LedMC, a subclass of both MotionCommand and LedEngine, performs this service. If we create an instance of LedMC, tell it the frequency at which to blink the LEDs, and add it to the MotionManager's list of active MCs, then it will do all the work for us. There's just one catch: our application is running in the Main process, while the MotionManager runs in a separate Motion process. This is necessary to assure that potentially lengthy computations taking place in Main don't prevent Motion from running every few milliseconds. So how can we communicate with our MotionCommand while at the same time making it available to the MotionManager?

The solution is to construct MotionCommands in a memory region that is shared by both processes. Because we have continuous access to the MotionCommand, we can change its parameters even while it's active, to tell it to do different things. But it's dangerous to modify a MotionCommand while the MotionManager is in the midst of invoking it. Therefore, Tekkotsu provides a mutual exclusion mechanism called an MMAccessor that temporarily locks out the MotionManager when we need to invoke a MotionCommand's member functions from within Main. Whenever we want to call such functions, we must lock down the MotionCommand by creating an MMAccessor first. Destroying the MMAccessor unlocks the MotionCommand.

There is one remaining wrinkle to the story. When a MotionCommand is passed to the MotionManager, it is assigned a unique ID called an MC_ID that identifies it within the MotionManager's active list. To lock the MotionCommand, we must pass this MC_ID value to the MMAccessor constructor. The MC_ID is also used when we tell the MotionManager to remove this MotionCommand from its active list. So the MC_ID must be saved somewhere. Normally it is kept in a protected data member within the Behavior instance so it can be shared by the doStart, doStop, and doEvent methods.

To summarize: MotionCommands must be instantiated in shared memory. An MC_ID, which is typically stored locally in the Behavior (not in shared memory), uniquely identifies the MotionCommand within the MotionManager's active list. Certain member functions of the MotionCommand will be called repeatedly from within the Motion process, by the MotionManager, to compute updated effector states. An MMAccessor, created in Main using the MC_ID, must be used to lock down an active MotionCommand so we can safely call its member functions from within the Main process. This allows us to modify the MotionCommand's parameters on the fly.

Exercise: Setting Up a MotionCommand to Cycle the LEDs

We will use a LedMC to make some LEDs blink, and increase the rate when a button is pressed. On the AIBO will use the face LEDs; for the Qwerkbot we'll use all the LEDs. First, we must create a data member to store the MC_ID of the MotionCommand we'll be creating, and initialize it in the DstBehavior constructor.

#include "Behaviors/StateMachine.h"

$nodeclass ButtonFlash : public StateNode :  leds_id(MotionManager::invalid_MC_ID) {

  MotionManager::MC_ID leds_id;  // id number for the MotionCommand

  ...

We use the global variable motman to access the MotionManager. doStart() instantiates a LedMC using the SharedObject<LedMC> constructor. It sets the LedMC's parameters to blink with a 1000 msec period, by calling its cycle(...) method. Then it adds the LedMC to the MotionManager's active list. Note that leds_mc is a local variable that will be lost when doStart() returns. That's okay because the actual MotionCommand is sitting in shared memory and is protected by a reference counter. The variable leds_mc is just a smart pointer to the shared memory region. The MotionCommand itself will not be deleted until it is removed from the MotionManager's active list and its reference count goes to zero.

  virtual void doStart() {
    cout << getName() << " is starting up." << endl;
    erouter->addListener(this,EventBase::buttonEGID);
    SharedObject<LedMC> leds_mc;
    leds_mc->cycle(RobotInfo::FaceLEDMask, 1000, 100.0);
    leds_id = motman->addPersistentMotion(leds_mc);
  }

The expresion leds_mc->cycle(...) above hides a syntactic trick: the cycle() function belongs to the nameless LedMC instance, not to the SharedObject stored at leds_mc. SharedObject overloads the -> operator and passes the call on to the LedEnging::cycle() function for the actual LedMC instance. Note: the variable RobotInfo::FaceLEDMask passed to cycle is defined in several model-specific namespaces, such as ERS210Info, ERS220Info, and ERS7Info. To write model-independent code, you can limit yourself to identifiers defined across all models, and refer to the generic RobotInfo namespace to automatically get the version appropriate to the model you're compiling for. Use RobotInfo::AllLEDMask to activate all the LEDs on a robot; this is a good choice for the Qwerkbot.

doStop() removes the LedMC instance from the Motion Manager's active list.

  virtual void doStop() {
    motman->removeMotion(leds_id);
    std::cout << getName() << " is shutting down." << endl;
  }

Inside doEvent, we will change the parameters of the running LedMC to alter its blink rate. To do this safely, we must first instantiate an MMAccessor, which we'll call leds_acc, using leds_id. This locks down the MotionCommand. Then we use the accessor to call the LedEngine::cycle method to change the blink period to 250 milliseconds for rapid blinking, or 1000 milliseconds to go back to slow blinking. The blink amplitude of 100.0 causes the LEDs to jump from full off to full on, or vice versa, instead of fading up or down gradually. (The cycle() function computes a sine wave. The LED amplitudes are bounded between 0 and 1.0, so multiplying the sine wave by 100.0 means the positive half of the wave will hit the 1.0 limit very quickly, producing a step function.)

Once again we are using a bit of syntactic sugar: MMAccessor overloads the -> operator to provide for method invocation, since we have no direct pointer to the LedMC instance which is sitting in shared memory. We do not have to explicitly unlock the MotionCommand: this happens automatically when we exit the block and leds_acc's destructor function is called.

  virtual void doEvent() {
    switch ( event->getGeneratorID() ) {

    case EventBase::buttonEGID: {
      cout << getName() << " got event: " << event->getDescription() << endl;
      MMAccessor<LedMC> leds_acc(leds_id);
      leds_acc->cycle(RobotInfo::FaceLEDMask, int(1000-750*event->getMagnitude()), 100.0);
      break; }

    default:
      cout << "Unexpected event:" << event->getDescription() << endl;
    }
  }

} // end of $nodeclass

REGISTER_BEHAVIOR(ButtonFlash);

Compile this code and boot the robot. Unpause it, and then activate ButtonFlash. The MotionCommand you created is now active, but no lights should be blinking because the default LedMC parameters tell it to do nothing. Now press a button and the call to cycle will update the parameters and cause the lights to blink at a rapid rate. Release the button and the lights should blink at a slower rate. Deactivate the Behavior and the MotionCommand will be removed from the active list.

Explore more:

  1. Skim the documentation for the MotionManager, MotionCommand, MMAccessor, and LedEngine classes.

  2. Besides cycling, what other effects can LedEngine provide?

  3. With your Behavior active, pause the robot by clicking on the "Stop" button or (if it's an AIBO) double clicking on the robot's back switch. This puts it into EStop (Emergency Stop) mode, which displays a pattern on the LEDs. You should see a mixture of the regular EStop pattern plus the cycle pattern being generated by your MotionCommand. Tekkotsu automatically blends the results of multiple MotionCommands that are trying to influence the same effector, if the commands are at the same priority level. How can setPriority() be used to obtain a different result if you don't want blending?


Detecting Motion Completion

Some MotionCommands run forever, unless explicitly removed from the active list. (LedMC will run forever if the parameters were set by cycle, but it will terminate automatically if the parameters were set by flash or cflash, which flashes the LEDs once.) A MotionCommand can signal that a motion has completed by posting a status event whose source ID is equal to the MotionCommand's MC_ID.

Exercise: Moving the Head and then Blinking

To get the robot to move its head, we use a HeadPointerMC motion command. In response to a button press, we will have the robot move its head up or down depending on the last commanded position. When the head motion completes, the robot will "blink" by flashing its LEDs.

#include "Behaviors/StateMachine.h"

$nodeclass MoveBlink : StateNode : leds_id(MotionManager::invalid_MC_ID),   \
		  head_id(MotionManager::invalid_MC_ID) {

  MotionManager::MC_ID leds_id, head_id;

  ...

Inside doStart() we set up the two motion commands, in a slightly more sophisticated way than in the previous example. We use the local variable leds_mc to hold the SharedObject that references the LedMC instance. With the HeadPointerMC, we create a SharedObject and then use the -> operator to invoke setMaxSpeed, setting the maximum speed for moving the head tilt joint to 0.5 radians/second. We don't need to construct an MMAccessor because the motion command has not yet been added to the active list, so the motion manager can't see it. After adding the head motion command to the motion manager's active list, we register a listener for status events from motmanEGID, with a source equal to our new motion command (referenced by head_id). The motion manager will post such an event each time our HeadPointerMC instance completes its assigned motion.

  virtual void doStart() {
    erouter->addListener(this,EventBase::buttonEGID);
    // set up LED motion command
    SharedObject<LedMC> leds_mc;
    leds_id = motman->addPersistentMotion(leds_mc);
    // set up head pointer motion command
    SharedObject<HeadPointerMC> head_mc;
    head_mc->setMaxSpeed(RobotInfo::TiltOffset,0.5);
    head_id = motman->addPersistentMotion(head_mc);
    erouter->addListener(this, EventBase::motmanEGID, head_id, EventBase::statusETID);
  }

  virtual void doStop() {
    motman->removeMotion(leds_id);
    motman->removeMotion(head_id);
  }

The logic for doEvent() is straightforward. Upon receiving a button press event, it gets the last commanded value for the head tilt from our HeadPointerMC instance by calling getJointValue(), and then chooses a new value so as to move the head to the complementary position, either lowpos or highpos. This new target position is commanded using setJointValue(). Note that lowpos and highpos are obtained from a table in the RobotInfo namespace, rather than being hard-coded constants. This allows the same code to work for both the ERS-2xx and ERS-7 models, which have different ranges of head motion. The variables outputRanges and HeadOffset are defined in Ers7Info.h (or ERS2xxInfo.h), and TiltOffset is defined in CommonInfo.h.

When the head motion completes, it throws a status event, and our program responds by telling the LedMC instance to flash the face LEDs for 500 milliseconds. When that motion command completes it will throw a status event of its own, which we could see if we had set up a listener for it.

  virtual void doEvent() {

    switch ( event->getGeneratorID() ) {

    case EventBase::buttonEGID:
      std::cout << getName() << " got button event: " << event->getDescription() << std::endl;
      if (event->getTypeID() == EventBase::activateETID) {
	const float lowpos = RobotInfo::outputRanges[HeadOffset+TiltOffset][MinRange];
	const float highpos = RobotInfo::outputRanges[HeadOffset+TiltOffset][MaxRange];
	const float midpos = (lowpos+highpos) / 2;
	MMAccessor<HeadPointerMC> head_acc(head_id);
	double val = head_acc->getJointValue(RobotInfo::TiltOffset);
	head_acc->setJointValue(RobotInfo::TiltOffset, (val > midpos) ? lowpos : highpos);
      }
      break;

    case EventBase::motmanEGID:
      std::cout << getName() << " got motman event: " << event->getDescription() << std::endl;
      { MMAccessor<LedMC> leds_acc(leds_id);
        leds_acc->flash(RobotInfo::FaceLEDMask, 500);
      }
      break; 

    default:
      std::cout << "Unexpected event:" << event->getDescription() << std::endl;
    }
  }

} // end of $nodeclass

Explore more:

  1. Suppose you push a button, the head starts to move, and you immediately push the button again. What will the head and LEDs do?

  2. When you first activate this behavior, the head moves to a neutral position. Why does it do that?

  3. Suppose you didn't want the head to move to a neutral postion on startup. To prevent this, try setting the motion command's initial target position equal to the head's current position. You can access the current tilt position using state->outputs[HeadOffset+TiltOffset]. You will need to include Shared/WordlState.h to do this.


Effector names

Tekkotsu uses several tables to describe the robot's joints and other effectors. For example, the maximum speed of each motor is stored in the array RobotInfo::MaxOutputSpeed[]. The software limits on range of motion of each joint are stored in an array of arrays named RobotInfo::outputRanges[][], which was referenced in the head motion example in the preceding section. The hard mechanical limits on joint motion are stored in a similar array named RobotInfo::mechanicalLimits[][]. The tables all use the same indexing scheme. The convention for referring to a joint in Tekkotsu is to specify its "offset" into these tables. Rather than writing actual integers, Tekkotsu provides enum types so you can express these offsets symbolically.

Here is how the numbering scheme works: Joints are grouped together in the table based on the robot's anatomy. Thus, all the leg joints have consecutive offsets, as do all the head joints, all the tail joints, and so on. The order of the legs is defined by RobotInfo::LegOrder_t, whose values are 0 to 3, defined symbolically as LFrLegOrder, RFrLegOrder, LBkLegOrder, and RBkLegOrder. The number of joints per leg is given by the constant RobotInfo::NumLegJoints, which is equal to 3 for current AIBO models. There are also symbols defined for each leg offset individually. So:

LFrLegOffset == LegOffset + LFrLegOrder * NumLegJoints
RFrLegOffset == LegOffset + RFrLegOrder * NumLegJoints
LBkLegOffset == LegOffset + LBkLegOrder * NumLegJoints
RBkLegOffset == LegOffset + RBkLegOrder * NumLegJoints

The order of joints within a leg is defined by RobotInfo::REKOffset_t, whose values are 0 to 2, defined symbolically as RotatorOffset, ElevatorOffset, and KneeOffset. (Note: the rotator and elevator joints are both located in the hip/shoulder; the rotator moves the leg forward or back, while the elevator moves it in or out.)

To refer to a specific leg joint, we start with the offset for the leg the joint is on, and add in the offset for the desired joint relative to that leg. So, for example, the offset for the right back knee is given by: RBkLegOffset + KneeOffset.

A similar convention applies to the head. The beginning of the head joints is specified by RobotInfo::HeadOffset. The joint order is defined by RobotInfo::TPROffset_t, which stands for Tilt, Pan, and Roll. (Note: the ERS-7 head does not roll. Instead, it has a second tilt joint called "nod". And the Qwerkbot has only tilt and pan.) The TPROffset values for the head joints are TiltOffset, PanOffset, and RollOffset (or NodOffset for the ERS-7.) So, for example, to access information about the head tilt joint we would use the index value HeadOffset+TiltOffset.

Joints are always referred to by their full indices, with one exception: the HeadPointerMC motion command, because it deals exclusively with the head, refers to joints by their TPROffset without adding in the value of HeadOffset. So if you want to set the head's pan angle using a HeadPointerMC, you would write something like:

head_acc->setJointValue(PanOffset, panvalue)
But if you wanted to set the pan angle as part of adjusting the robot's entire body posture, you would use a PostureMC rather than a HeadPointerMC, and you would refer to the pan joint using the full offset calculation:
posture_acc->setOutputCmd(HeadOffset+PanOffset, panvalue)

The PostureMC motion command is discussed further in the chapter on Posture and Motion Sequences.

Prev: Events
Up: Contents
Next: Postures and Motion Sequences

Last modified: Tue Feb 8 20:35:00 EST 2011