#include "Behaviors/StateMachine.h"
#include "Shared/mathutils.h" // for deg2rad function
using namespace mathutils;
$nodeclass MSeqTest : DynamicMotionSequenceNode : doStart {
float const leftGlanceAngle = deg2rad(60.0);
float const rightGlanceAngle = deg2rad(-70.0);
float const downGlanceAngle = deg2rad(-55.0);
MMAccessor<DynamicMotionSequence> mseq_acc = getMC();
// 1 sec to move head to look straight ahead
PostureEngine lookstraight;
lookstraight.setOutputCmd(HeadOffset+TiltOffset, 0.0);
lookstraight.setOutputCmd(HeadOffset+PanOffset, 0.0);
mseq_acc->advanceTime(1000);
mseq_acc->setPose(lookstraight);
mseq_acc->advanceTime(5000); // 5 secs to sit up
mseq_acc->LoadFile("situp.pos");
mseq_acc->advanceTime(1000); // 1 sec to glance left
mseq_acc->setOutputCmd(HeadOffset+PanOffset, leftGlanceAngle);
mseq_acc->advanceTime(2000); // hold glance for 2 secs
mseq_acc->setOutputCmd(HeadOffset+PanOffset, leftGlanceAngle);
mseq_acc->advanceTime(1000); // 1 sec to glance right
mseq_acc->setOutputCmd(HeadOffset+PanOffset, rightGlanceAngle);
mseq_acc->advanceTime(2000); // hold glance right for 2 secs
mseq_acc->setOutputCmd(HeadOffset+PanOffset, rightGlanceAngle);
mseq_acc->setOutputCmd(HeadOffset+TiltOffset, 0.0);// re-specify tilt in preparation for moving
mseq_acc->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_acc->setPose(currentpose);
mseq_acc->advanceTime(5000); // 5 secs to lie down
mseq_acc->LoadFile("liedown.pos");
mseq_acc->setOutputCmd(HeadOffset+TiltOffset, downGlanceAngle);
}
REGISTER_BEHAVIOR(MSeqTest);
|