Adding MotionCommand Functionality

This document will walk you through the creation of your first MotionCommand.  MotionCommands provide reusable motion primitives which are combined and controlled by a Behavior to accomplish a task.  This MotionCommand will set three of the legs to mirror the position of the fourth leg.

Prerequisites

1. You should have already completed the "Writing Your First MotionCommand" guide.

2. If you're new to C++, or just rusty, there is a C++ review available.

Adding Functionality

1. First, let's kill power to one of the legs so you can move it around by hand.  This is done by setting the PID values for the joint to 0.  PID control is a common method of making sure that when you tell an actuator to move to a position, it does so directly without (excessive) overshooting or oscillating.  The individual P, I, and D parameters are dependent on the actuator's characteristics and should not be modified individually without great caution.
   
   However, setting the PIDs to a percentage of the default value specified in the DefaultPIDs table is safe.  Setting them all to zero effectively "turns off" the joint.
   
   Your MotionCommand can control the PIDs by calling the setOutput() function.  This function takes an OutputPID class as the setting, but the OutputPID class can be implicitly constructed from a float[3], which makes the code a little more readable when you just want to use the default weight.  ("weight" is used to perform a weighted average if more than one MotionCommand is trying to set a joint's PIDs at the same time.)
   

   SampleMC.h

   // [...]
   #include "Motion/MotionManager.h"
    
   class SampleMC : public MotionCommand {
   public:
     // [...]
     virtual int updateOutputs() {
       float offpid[3]={0,0,0};
       for(unsigned int i=0; i<JointsPerLeg; i++)
         motman->setOutput(this,source+i,offpid);
     }
     // [...]
   }

   source is the index of the leg which we're going to be mirroring; offpid is just an array of floats set to 0.  We need to add source as a member field:
   
   SampleMC.h

   public:
     SampleMC() : MotionCommand(),
                  source(LFrLegOrder*JointsPerLeg+LegOffset)
     {}
     // [...]
   protected:
     unsigned int source;

   LFrLegOrder is defined (like most of these constants, if you haven't noticed already) in  the RobotInfo namespace, defined in RobotInfo.h.
   
   Notice we call setOutput() for each of the leg joints each time updateOutputs() is called.  If we don't call setOutput(), for a joint, it will automatically revert back to the default PID settings (or fall back on a lower-priority MotionCommand's settings).
   

2. Let's generalize a bit and add accessors so that we can control which joint is the source:
   

   SampleMC.h

   public:
     // [...]
     virtual void setSource(LegOrder_t leg) {
       source=leg*JointsPerLeg+LegOffset;
     }
     virtual LegOrder_t getSource() {
       return static_cast<LegOrder_t>((source-LegOffset)/JointsPerLeg);
     }

   Note that these functions aren't overriding anything, they're unique to this class.  You can create additional functions to allow behaviors to control your MotionCommands.
   

3. To use those functions, add the following lines to processEvents() of SampleBehavior:

   SampleBehavior.h

     virtual void processEvent(const EventBase& event) {
       if(event.getGeneratorID()==EventBase::buttonEGID) {
         //if it's a paw button, select that paw as the source in the
         //sample MC.  Normally i'd do this with one line:
         //MMAccessor<SampleMC>(mirror_id)->setSource(
         //              static_cast<LegOrder_t>(event.getSourceID()) );
         //...since these SIDs and Offsets just happen to have the same,
         //values but there's no intrinsic reason why they have to be the,
         //same so it would be more correct to say:
         switch(event.getSourceID()) {
         case LFrPawOffset:
           { MMAccessor<SampleMC>(mirror_id)->setSource(LFrLegOrder); }
           break;
         case RFrPawOffset:
           { MMAccessor<SampleMC>(mirror_id)->setSource(RFrLegOrder); }
           break;
         case LBkPawOffset:
           { MMAccessor<SampleMC>(mirror_id)->setSource(LBkLegOrder); }
           break;
        case RBkPawOffset:
           { MMAccessor<SampleMC>(mirror_id)->setSource(RBkLegOrder); }
           break;
         }
         // [...]
       } else {
         // [...]
       }
     }
   

   Now, when we press a paw button, SampleBehavior will receive the event and call the setSource() function.  Try it out.  You should be able to move freely whichever leg last had its paw button pushed.
   
   This is the last modification to SampleBehavior. SampleBehavior.h should look something like this.
   

4. Finally, the pièce de résistance: time to make the other 3 legs to mirror the source.
   
   In updateOutputs(), loop over all of the leg joints to record the positions of the corresponding joint on the source leg and then set the current joint to that value:

   SampleMC.h

   // [...]
   #include "Shared/WorldState.h"
   
   class SampleMC : public MotionCommand {
   public:
     // [...]
   
     virtual int updateOutputs() {
       for(unsigned int i=0; i<NumLegJoints; i++) {
         unsigned int joint=i%JointsPerLeg;
         float source_pos=state->outputs[source+joint];
         motman->setOutput(this,LegOffset+i,source_pos);
       }
       // [...]
     }
     
     // [...]
   }
   

   Now try it out - the other joints should match the movements of the source leg, like a puppet.
   
   Note: It is entirely possible to make the legs hit each other or set them to conflicting positions.  Keep this in mind and be careful.  Our software does not (yet) have any means for detecting "binds".  This could possibly burn out a motor if you aren't watching the robot to free it or turn on e-stop and debug it.
   
   state is a global from WorldState, and holds information regarding the current joint positions, LED status, sensor readings, etc.
   
   Notice that the setOutputs() function is overloaded - this time we're passing joint angles.  Passing a single float value implicitly converts it to a OutputCmd with weight 1, which is what is actually passed to the MotionManager.
   

5. All done!  Your SampleMC should look something like this, and SampleBehavior should look like this.

Further Exercises

1. Does it seem like the movements of the other arms are a little jerky?  It is possible to make them smoother.  The OS takes output values in groups, currently 4 at a time. (this value is defined in NumFrames)  Each frame lasts FrameTime (8) milliseconds.  So each buffer consists of (currently) 32 milliseconds worth of commands.  The commands are double buffered, (one buffer is being executed by the system while the motion process fills out the new one.) so lag time is at least 32-64 ms.
   
   When we call setOutput() with a single value, this value is replicated across thes 4 frames.  Try using some velocity information to predict future movement and pass this as an array of OutputCmd[NumFrames].