Adding MotionCommand Functionality
will walk you through the creation of your first MotionCommand.
MotionCommands provide reusable motion primitives which are combined
controlled by a Behavior to accomplish a task.
You should have already completed the "Writing Your First MotionCommand"
- If you're new to C++, or just rusty, there is a C++ review available.
This MotionCommand will cause the joint angles of one selected leg to be mirrored to all the other legs and cause them to track its motion.
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.
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:
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"
Your MotionCommand can control the PIDs by calling the setOutput() function.
function takes an OutputPID
as the setting, but the OutputPID class can be implicitly constructed
from a float, 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
more than one MotionCommand is trying to set a joint's PIDs at the same
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).
Let's generalize a bit and add accessors so that we can
control which joint is the source:
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:
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".
could possibly burn out a motor if you aren't watching the robot to
it or turn on e-stop and debug it.
state is a
from WorldState, and holds
information regarding the current joint positions, LED status, sensor
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.