01/26/08 15-494 Cognitive Robotics 1Motion Commands andReal-Time Programming15-494 Cognitive RoboticsDavid S. Touretzky &Ethan Tira-ThompsonCarnegie MellonSpring 200801/26/08 15-494 Cognitive Robotics 2Motion CommandsLive in Shared Memory01/26/08 15-494 Cognitive Robotics 3Motion Commands Are ObjectsA MotionCommand is an object with 2 kinds of methods:1) Command methods for telling it what you want it to do.–Called by user code running in Main.2) An updateOutputs() method for computing new effector values (joint angles, LED brightness, etc.)–Called every 32 ms by the motion manager, running in Motion.01/26/08 15-494 Cognitive Robotics 4Types of Motion CommandsRed outline means this class has subclasses.01/26/08 15-494 Cognitive Robotics 5Creating a Motion Command●SharedObject<LedMC> leds_mc;●The actual LedMC object is created in shared memory.●The SharedObject named leds_mc lives in Main's address space, and holds a pointer to the shared memory region.●Two ways to refer to a motion command within Main:–via the shared object–via the MC_ID (Motion Command ID) assigned to it by the Motion Manager (motman) when the motion command is active01/26/08 15-494 Cognitive Robotics 6LedMC●Defined in Motion/LedMC.h●LedMC inherits from two parent classes.●MotionCommand:–updateOutputs()–isAlive() : is this command active?–isDirty() : have outputs changed?●LedEngine:–cycle(...) : cycle these LEDs (sine wave pattern)–flash(...) : flash these LEDs for n msecs, then end–invert(...) : invert the status of these LEDs–etc.01/26/08 15-494 Cognitive Robotics 7LedEngine●cycle(LEDBitmask_t bitmask,unsigned int period,float amplitude,float offset=0,int phase=0)period = 5000 msamplitude = 1period = 5000 msamplitude = 5offset = -1period = 2000 msamplitude = 20001/26/08 15-494 Cognitive Robotics 8Sample LedMC Program#include "Behaviors/BehaviorBase.h"#include "Motion/LedMC.h"#include "Motion/MotionManager.h"class DstBehavior : public BehaviorBase {protected: MotionManager::MC_ID leds_id; // id of MotionCommandpublic: DstBehavior() : BehaviorBase("DstBehavior"), leds_id(MotionManager::invalid_MC_ID) {}01/26/08 15-494 Cognitive Robotics 9Sample LedMC Program virtual void DoStart() { BehaviorBase::DoStart(); cout << getName() << " is starting up." << endl; SharedObject<LedMC> leds_mc; leds_mc->cycle(RobotInfo::FaceLEDMask, 1000, 100.0); leds_id = motman->addPersistentMotion(leds_mc); }●SharedObjects and MCs are both reference counted.●What happens to leds_mc when DoStart returns?●What happens to the motion command?01/26/08 15-494 Cognitive Robotics 10Operator Overloading●leds_mc is of type SharedObject●cycle(...) is a method of LedEngine, not SharedObject.●So why does this work?leds_mc -> cycle(RobotInfo::FaceLEDMask, 1000, 100.0);●The arrow operator is overloaded by SharedObject. It will dereference the pointer to the actual LedMC in shared memory, and call its cycle(...) method.01/26/08 15-494 Cognitive Robotics 11Sample LedMC Programvirtual void DoStop() { motman->removeMotion(leds_id); leds_id = MotionManager::invalid_MC_ID; cout << getName() << " is shutting down." << endl; BehaviorBase::DoStop(); }●We needed to keep leds_id around so we could reference the motion command in DoStop().●You should always remove motion commands when you're done with them, unless autopruned.●cycle() can't be autopruned. Why not?01/26/08 15-494 Cognitive Robotics 12Mutual Exclusion: MMAccessor●Suppose we want to change the parameters of a motion command while it's active.●Example: change the cycle period of a LedMC.●Not safe for Main to change an active MC while Motion is trying to use it. Need a mutex mechanism: { MMAcessor<LedMC> leds_acc(leds_id); leds_acc->cycle(RobotInfo::FaceLEDMask, 250, 100.0);}●Constructor handles checkout; destructor handles checkin. Within scope of leds_acc, motman locked out.●Don't lock it out for too long!01/26/08 15-494 Cognitive Robotics 13Changing the Cycle PeriodWhen a Button Is Pressedvirtual void DoStart() { BehaviorBase::DoStart(); cout << getName() << " is starting up." << endl; SharedObject<LedMC> leds_mc; leds_mc->cycle(RobotInfo::FaceLEDMask, 1000, 100.0); leds_id = motman->addPersistentMotion(leds_mc); erouter->addListener(this, EventBase::buttonEGID, RobotInfo:LFrPawOffset); }01/26/08 15-494 Cognitive Robotics 14Changing the Cycle PeriodWhen a Button Is Pressedvirtual void processEvent(const EventBase &event) { int const new_period = event.getMagnitude() == 0 ? 1000 : 250; MMAccessor<LedMC> leds_acc(leds_id); leds_acc->cycle(RobotInfo::FaceLEDMask,new_period,100.0); }01/26/08 15-494 Cognitive Robotics 15Using MMAccessors●Declare a local variable if you need to change multiple MC parameters: MMAccessor<LedMC> leds_acc(leds_id); leds_acc->cycle(RobotInfo::FaceLEDMask,500,1.0); leds_acc->cycle(RobotInfo::BackLEDMask,2000,2.0);●Just call the constructor if you only need to change one: MMAccessor<LedMC>(leds_id)-> cycle(RobotInfo::FaceLEDMask,500,1.0);01/26/08 15-494 Cognitive Robotics 16Prunable Motions●flash(LEDBitMask_t bitmask, float value, unsigned int msec)Sets the specified LEDs to value for so many msec, then sets them back.●Once the action is complete, the motion command has no more work to do.●If it's a persistent motion, it sits around waiting for its next assignment. If a prunable motion, the motion manager removes (prunes) it.01/26/08 15-494 Cognitive Robotics 17Flash the Back LEDs for 15 secsvirtual void DoStart() { BehaviorBase::DoStart(); SharedObject<LedMC> leds_mc; leds_mc->flash(RobotInfo::BackLEDMask, 15000, 1.0); leds_id = motman->addPrunableMotion(leds_mc); cout << “Created LedMC, id = “ << leds_id << endl; BehaviorBase::DoStop(); }●What would happen if you started this behavior three times within a few seconds?01/26/08 15-494 Cognitive Robotics 18Moving the Head●Three head joints: tilt, pan, nod●Head joints are namedby their offsets into the joint array: TiltOffset PanOffset NodOffset01/26/08 15-494 Cognitive Robotics 19The Camera Defines the “Head”Qwerkbot: 2DOF “head”(pan and tilt)Regis: 4DOF “goose neck”:base (pan), shoulder/elbow/wrist (tilt)01/26/08 15-494 Cognitive Robotics 20HeadPointerMCDefined in Motion/HeadPointerMC.h●void setJointValue(unsigned int joint, float value)–setJointValue(TiltOffset, 0.5)●float getJointValue(unsigned int joint) const●void setMaxSpeed(unsigned int