Tekkotsu

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.

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 something like the following to processEvents() of SampleBehavior. This assumes you have already called erouter->addListener to subscribe to button events as shown in the behavior tutorial.

   SampleBehavior.h

   virtual void processEvent(const EventBase& event) {
   

   if(event.getGeneratorID()==EventBase::buttonEGID) {
   

   // [...]
   

   // this is a bit of a hack to map from button IDs to
   // whatever happens to be the corresponding numeric leg order
   mirror->setSource(event.getSourceID());
   

   // [...]
   

   } /* 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 legs to mirror the source leg.
   
   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.