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.
You should have already completed the "Writing Your First MotionCommand" guide.
It will illustrate how to control individual joints, the use of modifying PID parameters to “disable” joints, and interaction with behaviors to receive new parameters (i.e. which leg to use as the “source”). If your robot does not have legs (or does not have joint sensing), you may have to modify portions of this tutorial.
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.)
// [...]#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);
// [...]
}
public:
SampleMC() : MotionCommand(),
{}
// [...]
protected:unsigned int source;
Let's generalize a bit and add accessors so that we can
control which joint is the source:
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.
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.
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.
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:
// [...]#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.
All done! Your SampleMC should look something like this, and SampleBehavior should look like this.