#ifndef INCLUDED_DstBehavior_h_
#define INCLUDED_DstBehavior_h_
#include "Behaviors/BehaviorBase.h"
#include "Motion/MotionManager.h"
#include "Motion/MotionSequenceMC.h"
#include "Shared/mathutils.h" // for deg2rad function
using namespace mathutils;
class DstBehavior : public BehaviorBase {
public:
DstBehavior() : BehaviorBase("DstBehavior") {}
virtual void DoStart() {
BehaviorBase::DoStart();
float const leftGlanceAngle = deg2rad(60.0);
float const rightGlanceAngle = deg2rad(-70.0);
float const downGlanceAngle = deg2rad(-55.0);
SharedObject<MediumMotionSequenceMC> mseq_mc;
// 1 sec to move head to look straight ahead
PostureEngine lookstraight;
lookstraight.setOutputCmd(HeadOffset+TiltOffset, 0.0);
lookstraight.setOutputCmd(HeadOffset+PanOffset, 0.0);
mseq_mc->advanceTime(1000);
mseq_mc->setPose(lookstraight);
mseq_mc->advanceTime(5000); // 5 secs to sit up
mseq_mc->LoadFile("situp.pos");
mseq_mc->advanceTime(1000); // 1 sec to glance left
mseq_mc->setOutputCmd(HeadOffset+PanOffset, leftGlanceAngle);
mseq_mc->advanceTime(2000); // hold glance for 2 secs
mseq_mc->setOutputCmd(HeadOffset+PanOffset, leftGlanceAngle);
mseq_mc->advanceTime(1000); // 1 sec to glance right
mseq_mc->setOutputCmd(HeadOffset+PanOffset, rightGlanceAngle);
mseq_mc->advanceTime(2000); // hold glance right for 2 secs
mseq_mc->setOutputCmd(HeadOffset+PanOffset, rightGlanceAngle);
mseq_mc->setOutputCmd(HeadOffset+TiltOffset, 0.0); // re-specify tilt in preparation for moving
mseq_mc->advanceTime(1000); // 1 sec to glance down
PostureEngine currentpose("situp.pos"); // update body joint time indices
currentpose.setOutputCmd(HeadOffset+TiltOffset, downGlanceAngle); // and tilt the head down
mseq_mc->setPose(currentpose);
mseq_mc->advanceTime(5000); // 5 secs to lie down
mseq_mc->LoadFile("liedown.pos");
mseq_mc->setOutputCmd(HeadOffset+TiltOffset, downGlanceAngle);
motman->addPrunableMotion(mseq_mc);
DoStop();
}
};
#endif
|