Motion Commands, Postures, and Sequences

Robotics Seminar CSI445/660Spring 2006Robert SalkinUniversity at Albany, SUNY

A few points from HW2

You can get data out of an event getGeneratorID, getSourceID, getTypeID Probably more reliable than checking state-

>buttons[] for a value You want the event, not the value

The button was pressed if an event happened May not still be pressed when the button value is

checked Not always true

Sensor values Buttons acting as sensors

may be pressed harder or softer without activate/deactivate type events being triggered

Flags are helpful

Use flags during event processing head=!head

Flips value of flag when head is pressed

Update physical state of AIBO only if an update is needed Is the AIBO already stopped (or moving)?

Changing the physical state in one place can avoid conflicting (or repetitive) instructions Jerky movement

Getting Started

The Basic Problem [1]: We have n joints, each with a desired position which we

have specified Each joint has an actuator which is given a command in

units of torque Most common method for determining required torques is

by feedback from joint sensors PID (Proportional Integral Differential) Control The solution to this problem is based in forward and

inverse kinematics for legged locomotion. Fortunately, Tekkotsu abstracts all of that for us!!!

MotionCommands

Tekkotsu provides several primitive motions that can be used in our behaviors WalkMC

Uses the CMPack engine to resolve and apply PID calculations on a 51 parameter set to achieve forward, reverse, and angled walking.

HeadPointerMC Uses a three parameter set (tilt, pan, nod) to control the

direction of Aibo’s head. TailWagMC

uses period, magnitude, and tilt to wag Aibo’s tail. The first 2 are used in most behaviors

Adding motion to a behavior

Before diving into the structural details of writing our own MotionCommands, let’s take another look at WalkForwardAndStop to see how to implement these “black box” motions. For each motion we wish to add, a unique ID needs to be

created in our behavior and assigned for it MotionManager::MC_ID walk_id

MotionCommands are part of the shared memory region, and need to be instantiated as such:

walk_id = motman->addPersistentMotion(SharedObject<WalkMC>());

Each motion needs a variable to provide access at the time a change needs to be made:

MMAccessor<WalkMC> walk(walk_id);

WalkMC

WalkMC. Use the setTargetVelocity method to effect changes.

walk.mc()->setTargetVelocity( x,y,a);

x is positive in the forward direction y is orthogonal to x. (left is positive) a is the angular velocity and is the means by

which to create turns. positive in the counter-clockwise direction.

walk.mc()->setTargetVelocity(150.0, 0, 0) causes a straight forward walk

walk.mc()->setTagetVelocity(0,0,0) stops AIBO on the next update

up to 64ms lag time

x

y

a

HeadPointer MC

HeadPointerMC works very similarly to walkMC.

head.mc()->setJoints(tilt, pan, nod)

tilt – broad vertical joint RAD(-75) to RAD(0)

pan – horizontal joint RAD(-88) to RAD(88)

nod (roll on the 210) - fine vertical joint

RAD(-15) to RAD(45)

pan

tilt

nod

TailWagMC

Uses a period (frequency) and magnitude to make the little tail wag

Strictly a cuteness / realism MC Should this be implemented as a background

behavior? Depends on how long (or when) we want the tail

to wag…

MotionCommands

The Tekkotsu MotionCommand is the construct by which we can create motions on the Aibo.

Implemented as a class NOT a behavior! Take a look at:

Writing your first MotionCommand http://www-2.cs.cmu.edu/~tekkotsu/

FirstMotionCommand.html MotionCommand Class Reference

Tekkotsu.org Reference When Tekkotsu spawns, it creates (among other things), one

process named MMCombo which is then “forked” (by renaming, bit manipulation, and careful branching) into MainObj and MotoObj.

As a result, there are some concerns when working with MotionCommands…

Warnings

Be careful what you call in MotionManager“Some functions are marked MotionCommand-safe - this is another issue due to our "fake" fork. In short, when a function is called on a MotionCommand, it uses the context of whatever process created it, not the process that actually made the function call. Thus, when Motion calls updateOutputs(), calls that the MotionCommand makes are indistinguishable from concurrent calls from Main. This can cause deadlock if a function is called which locks the MotionManager. To get around this, we need to pass the 'this' parameter on functions that require a lock, namely MotionManager::setOutputs(). This allows the MotionManager to figure out that it's the same MotionCommand it previously called updateOutputs() on, and thus avoid trying to lock itself again.”

Warnings

Don't store pointers in motion commands! “Since motion commands are in shared memory, and these shared memory regions can have different base pointers in each process, pointers will only be valid in the process from which they were assigned. In other processes, that address may point to something else, especially if it was pointing outside of the shared memory regions.There are convoluted ways of getting around this. If needed, MotionManager could be modified to hand out shared memory regions upon request. Let's try to avoid this for now. Keep MotionCommands simple, without dynamic memory. Do more complicated stuff with behaviors, which only have to worry about running in Main.”

MotionCommand Class

#ifndef INCLUDED_SampleMC_h_#define INCLUDED_SampleMC_h_

#include "Motion/MotionCommand.h"

class SampleMC : public MotionCommand {public: // Constructor SampleMC() : MotionCommand() {}

// Abstract functions: // These must be defined by every MotionCommand virtual int updateOutputs() {} virtual int isDirty() {} virtual int isAlive() {}};

#endif

Implementing

updateOutputs() called once per "cycle".  This is where joint updates actually take place. It should return the number of "dirty" outputs. ("dirty"

means outputs which have changed since the last cycle.) Currently the return value is unused, but in the future

MotionManager may use this information to avoid redundant calculations. 

If this would be expensive to calculate precisely, at least be sure to return a non-zero value if there are any dirty outputs.

Implementing

isDirty() not called at present, but should return the same value as

updateJointCmds() .  But just for example, we'll only return true for now.

isAlive() should return true as long as the MotionCommand is active.  If this returns false and the MotionCommand is set to autoprune,

MotionManager will remove it by itself.  This is handy for "throw away" motion primitives.

Take a look at the SampleMC tutorial for a good look at implementing MCs from scratch:

http://www-2.cs.cmu.edu/~tekkotsu/FirstMotionCommand.html

updateOutputs

It’s called automatically once the motion command is “active”. Therefore we never need call it directly from the behavior create get and set methods for Motion Commands to give

behaviors control over motion parameters joint updates are effected here through

motman->setOutput(this, source, value) source is a constant for the desired joint as defined ERS7Info value is the “weight” we wish to set for that joint note that setOutput is overloaded and behaves slightly differently

depending on how it’s used. look at the documentation for MotionManager value can be a single value or an OutputCMD to hold multiple frames

of data

MotionSequences

In addition to creating flexible MotionCommands, Tekkotsu provides a simple means of executing “canned” (pre-determined) motions. convenient for things that are very repetitious or don’t rely

on feedback from the joints or the world AutoGetup HeadScan (maybe) some kicks

MotionSequences are built as small script files that get loaded and played on Aibo at run time

Be cautious of joints specified so they don’t conflict with a running behavior when could this happen?

MotionSequence Files

The format is straightforward and ASCII text:#MSq (first line)

Zero or more of:

advanceTime time (new name for delay)

outputname value [weight] (sets the specified output to the value - assumes 1 for weight)

load filename (file is a posture, sets position)

overlay filename (file can be a posture or another motion sequence)

Degrees (following <value>s will be interpreted as degrees [default])

radians (following <value>s will be interpreted as radians)

#END (last line -- add a newline after it)

Sequence Files

A very simple example of a valid motion sequence:

#MSq degrees advanceTime 500 NECK:pan~ 0 advanceTime 500 NECK:pan~ 65 advanceTime 500 NECK:pan~ 0 advanceTime 500 NECK:pan~ -65 advanceTime 500 NECK:pan~ 0 NECK:pan~ 0

0 #END

So a very simple example of a valid motion sequence would be:

start fileangles are in degreeswait half secondzero neck jointwaitmove counter-clockwisewaitmove to centerwaitmove clockwisewaitmove to centerfade-out jointend file

More Sequence

Lines beginning with '#' are ignored. Output names are defined in ERS7Info

makes sure to check angle limits of joints in the robot info file.

Take a look in project/ms/data/motion for some good examples

Sequences are added to the motion queue much like Motion Commands

It’s probably a good idea to provide behaviors a mechanism for detecting when a motion sequence has ended avoid overlaps & conflicts

Usage

use config->motion.makepath(motionfile) to return a std::string with a resolved pathname to motion.root.

std::string mymot = config->motion.makepath(“kick.mot”);

create a shared object passing the full path to the constructor SharedObject< MotionSequenceMC<MotionSequence::SizeMedium>

>kick (mymot.c_str()); add the motion to the manager

MotionManager::MC_ID id=motman->addMotion(kick,MotionManager::kStdPriority);

Keep high priority for “interrupting” type issues (auto-getup) we can use the event handler to inform when the motion has

finished erouter-

>addListener(this,EventBase::motmanEGID,id,EventBase::deactivateETID);

Postures

Postures (as the name implies) allow the setting of many joints in a single frame (simultaneously) to achieve the impression of well, a posture.

Implemented very much like Motion Sequences : Create an object identically to a Motion Sequence Instead of setting the filename in the constructor, we use the setPose() method of the Motion Sequence class to use the “pose” features.

situp->setPose(PostureEngine(mymot.c_str())); Where situp is a MotionSequenceMC

use the setPlayTime(ms) method to set the duration (in ms) of the entire run-time for the posture.

This is a way to achieve smooth looking, or rapid motion postures Used for things like standing, laying down, sitting, etc.

Intended to move once and stop Contrast to a sequence where one or more joints normally have a

visible pause between movements; all joints in a posture appear to move together

Posture file

Like a sequence, postures are created in a script file: The format is extremely simple:

Header line (mandatory) #POS

One or more: Joint target-angle (rads) weight

LFr:rotor -0.392146 1.000000 Close line (mandatory)

#END When the PostureEngine constructor is run it parses this file

into a series of OutputCmd’s which are executed (and averaged into other motion requests) when play() is called, or the Motion is added to motman ( play() is implied there )

More Postures

We can use the pause() method of MotionSequenceMC prior to adding the posture to motman to prevent it from running right away.   situp->setPlayTime(700);    situp->setPose(PostureEngine(

"/ms/data/motion/situp.pos"));   situp->pause();

…. somewhere later situp_id = motman->addPrunableMotion(situp,                 MotionManager::kStdPriority,

false);

Deprecated addMotion

addMotion is deprecated addPrunableMotion

Motion that has a distinct endpoint

addPersistentMotion Motion that has no distinct endpoint

Listen for motmanEGID event with your motion_id as the SourceID to know when a motion ends and another may begin

Sound

For HW3, you’ll need to play a sound Simply include the header

#include “SoundPlay/SoundManager.h” Load the file in DoStart()

sndman->LoadFile(“file.wav");

When you want the sound to play sndman->PlayFile(“file.wav”);

Release the file in DoStop() sndman->ReleaseFile("barkhigh.wav");

Important Links

Walking http://www.cs.cmu.edu/~dst/Tekkotsu/Tutorial/

walking.shtml Postures/Motion Sequences

http://www.cs.cmu.edu/~dst/Tekkotsu/Tutorial/postures.shtml

Playing Sounds http://www.cs.cmu.edu/~dst/Tekkotsu/Tutorial/

sound.shtml

References

CMU Robobits Week 3 lecture “Motion” http://www-2.cs.cmu.edu/~robosoccer/cmrobobits/

Shawn Turner