int main() { StateMachine stateMachine; stateMachine.setOnStartCallback([] () { std::cout << "state machine started" << std::endl; }); stateMachine.setOnStopCallback([] () { std::cout << "state machine stopped" << std::endl; }); State* state1 = new State(); state1->setOnEnterCallback([] (const Event*) { std::cout << "entering state1" << std::endl; }); state1->setOnExitCallback([] (const Event*) { std::cout << "exiting state1" << std::endl; }); State* state2 = new State(); state2->setOnEnterCallback([] (const Event*) { std::cout << "entering state2" << std::endl; }); state2->setOnExitCallback([] (const Event*) { std::cout << "exiting state2" << std::endl; }); State* state3 = new State(); state3->setOnEnterCallback([] (const Event*) { std::cout << "entering state3" << std::endl; }); state3->setOnExitCallback([] (const Event*) { std::cout << "exiting state3" << std::endl; }); stateMachine.addTransition(12, state1, state2); stateMachine.addTransition(13, state1, state3); stateMachine.addTransition(23, state2, state3); stateMachine.addTransition(31, state3, state1); stateMachine.setInitialState(state1); stateMachine.start(); stateMachine.postEvent(new Event(12)); stateMachine.postEvent(new Event(23)); stateMachine.postEvent(new Event(31)); stateMachine.postEvent(new Event(13)); return 0; }
int main(int argc, char * argv[]) { StateMachine * machine = new StateMachine(); View001 * view001 = new View001(); view001->addTransition(machine); View002 * view002 = new View002(); view002->addTransition(machine); machine->addState(view001); machine->addState(view002); machine->setInitialState(view001); machine->start(); return 0; }
//============================================================================== StateMachine* Controller::_createStandingStateMachine() { using namespace dart::math::suffixes; StateMachine* standing = new StateMachine("standing"); State* standingState0 = new State(mAtlasRobot, "0"); TerminalCondition* tcStanding0 = new TimerCondition(standingState0, 0.3); standingState0->setTerminalCondition(tcStanding0); standingState0->setNextState(standingState0); standingState0->setDesiredJointPosition( "back_bky", 15.00_deg); // angle b/w pelvis and torso standingState0->setDesiredJointPosition("l_leg_hpy", -10.00_deg); standingState0->setDesiredJointPosition("r_leg_hpy", -10.00_deg); standingState0->setDesiredJointPosition("l_leg_kny", 30.00_deg); // left knee standingState0->setDesiredJointPosition("r_leg_kny", 30.00_deg); // right knee standingState0->setDesiredJointPosition( "l_leg_aky", -16.80_deg); // left ankle standingState0->setDesiredJointPosition( "r_leg_aky", -16.80_deg); // right ankle standingState0->setDesiredJointPosition( "l_arm_shx", -90.0_deg); // right ankle standingState0->setDesiredJointPosition( "r_arm_shx", +90.0_deg); // right ankle standing->addState(standingState0); standing->setInitialState(standingState0); return standing; }
//============================================================================== StateMachine* Controller::_createRunningStateMachine() { using namespace dart::math::suffixes; const double cd = 0.5; const double cv = 0.2; const double pelvis = -10.0_deg; // angle b/w pelvis and torso const double swh01 = 0.50; // swing hip const double swk01 = -1.10; // swing knee const double swa01 = 0.60; // swing angle const double stk01 = -0.05; // stance knee const double sta01 = 0.00; // stance ankle StateMachine* sm = new StateMachine("running"); State* state0 = new State(mAtlasRobot, "0"); State* state1 = new State(mAtlasRobot, "1"); TerminalCondition* cond0 = new TimerCondition(state0, 0.15); TerminalCondition* cond1 = new TimerCondition(state1, 0.15); state0->setTerminalCondition(cond0); state1->setTerminalCondition(cond1); state0->setNextState(state1); state1->setNextState(state0); // Set stance foot state0->setStanceFootToLeftFoot(); state1->setStanceFootToRightFoot(); // Set global desired pelvis angle state0->setDesiredPelvisGlobalAngleOnSagital(0.0_deg); state1->setDesiredPelvisGlobalAngleOnSagital(0.0_deg); state0->setDesiredPelvisGlobalAngleOnCoronal(0.0_deg); state1->setDesiredPelvisGlobalAngleOnCoronal(0.0_deg); // Set desired joint position //-- State 0 //---- pelvis state0->setDesiredJointPosition( "back_bky", -pelvis); // angle b/w pelvis and torso //---- swing leg state0->setDesiredJointPosition("r_leg_hpy", -swh01); // right hip state0->setDesiredJointPosition("r_leg_kny", -swk01); // right knee state0->setDesiredJointPosition("r_leg_aky", -swa01); // right ankle //---- stance leg state0->setDesiredJointPosition("l_leg_kny", -stk01); // left knee state0->setDesiredJointPosition("l_leg_aky", -sta01); // left ankle //---- arm state0->setDesiredJointPosition("l_arm_shy", -45.00_deg); // left arm state0->setDesiredJointPosition("r_arm_shy", +15.00_deg); // right arm state0->setDesiredJointPosition("l_arm_shx", -80.00_deg); // left arm state0->setDesiredJointPosition("r_arm_shx", +80.00_deg); // right arm // state0->setDesiredJointPosition(23, DART_RADIAN * +90.00); // left arm // state0->setDesiredJointPosition(24, DART_RADIAN * +90.00); // right arm // state0->setDesiredJointPosition(27, DART_RADIAN * +90.00); // left arm // state0->setDesiredJointPosition(28, DART_RADIAN * -90.00); // right arm //---- feedback gain for hip joints state0->setFeedbackCoronalCOMDistance( mCoronalLeftHip, -cd); // coronal left hip state0->setFeedbackCoronalCOMVelocity( mCoronalLeftHip, -cv); // coronal left hip state0->setFeedbackCoronalCOMDistance( mCoronalRightHip, -cd); // coronal right hip state0->setFeedbackCoronalCOMVelocity( mCoronalRightHip, -cv); // coronal right hip state0->setFeedbackSagitalCOMDistance( mSagitalLeftHip, -cd); // sagital left hip state0->setFeedbackSagitalCOMVelocity( mSagitalLeftHip, -cv); // sagital left hip state0->setFeedbackSagitalCOMDistance( mSagitalRightHip, -cd); // sagital right hip state0->setFeedbackSagitalCOMVelocity( mSagitalRightHip, -cv); // sagital right hip //-- State 2 //---- pelvis state1->setDesiredJointPosition( "back_bky", -pelvis); // angle b/w pelvis and torso //---- swing leg state1->setDesiredJointPosition("l_leg_hpy", -swh01); // left hip state1->setDesiredJointPosition("l_leg_kny", -swk01); // left knee state1->setDesiredJointPosition("l_leg_aky", -swa01); // left ankle //---- stance leg state1->setDesiredJointPosition("r_leg_kny", -stk01); // right knee state1->setDesiredJointPosition("r_leg_aky", -sta01); // right ankle //---- arm state1->setDesiredJointPosition("l_arm_shy", +15.00_deg); // left arm state1->setDesiredJointPosition("r_arm_shy", -45.00_deg); // right arm state1->setDesiredJointPosition("l_arm_shx", -80.00_deg); // left arm state1->setDesiredJointPosition("r_arm_shx", +80.00_deg); // right arm // state1->setDesiredJointPosition(23, DART_RADIAN * +90.00); // left arm // state1->setDesiredJointPosition(24, DART_RADIAN * +90.00); // right arm // state1->setDesiredJointPosition(27, DART_RADIAN * +90.00); // left arm // state1->setDesiredJointPosition(28, DART_RADIAN * -90.00); // right arm //---- feedback gain for hip joints state1->setFeedbackCoronalCOMDistance( mCoronalLeftHip, -cd); // coronal left hip state1->setFeedbackCoronalCOMVelocity( mCoronalLeftHip, -cv); // coronal left hip state1->setFeedbackCoronalCOMDistance( mCoronalRightHip, -cd); // coronal right hip state1->setFeedbackCoronalCOMVelocity( mCoronalRightHip, -cv); // coronal right hip state1->setFeedbackSagitalCOMDistance( mSagitalLeftHip, -cd); // sagital left hip state1->setFeedbackSagitalCOMVelocity( mSagitalLeftHip, -cv); // sagital left hip state1->setFeedbackSagitalCOMDistance( mSagitalRightHip, -cd); // sagital right hip state1->setFeedbackSagitalCOMVelocity( mSagitalRightHip, -cv); // sagital right hip sm->addState(state0); sm->addState(state1); sm->setInitialState(state0); return sm; }
//============================================================================== StateMachine* Controller::_createWalkingInPlaceStateMachine() { using namespace dart::math::suffixes; const double cd = 0.5; const double cv = 0.2; const double pelvis = -4.75_deg; // angle b/w pelvis and torso const double swh02 = 0.50; // swing hip const double swk02 = -1.10; // swing knee const double swa02 = 0.60; // swing angle const double stk02 = -0.05; // stance knee const double sta02 = 0.00; // stance ankle const double swh13 = -0.10; // swing hip const double swk13 = -0.05; // swing knee const double swa13 = 0.15; // swing angle const double stk13 = -0.10; // stance knee const double sta13 = 0.00; // stance ankle StateMachine* sm = new StateMachine("walking in place"); State* state0 = new State(mAtlasRobot, "0"); State* state1 = new State(mAtlasRobot, "1"); State* state2 = new State(mAtlasRobot, "2"); State* state3 = new State(mAtlasRobot, "3"); TerminalCondition* cond0 = new TimerCondition(state0, 0.3); TerminalCondition* cond1 = new BodyContactCondition(state1, _getRightFoot()); TerminalCondition* cond2 = new TimerCondition(state2, 0.3); TerminalCondition* cond3 = new BodyContactCondition(state3, _getLeftFoot()); state0->setTerminalCondition(cond0); state1->setTerminalCondition(cond1); state2->setTerminalCondition(cond2); state3->setTerminalCondition(cond3); state0->setNextState(state1); state1->setNextState(state2); state2->setNextState(state3); state3->setNextState(state0); // Set stance foot state0->setStanceFootToLeftFoot(); state1->setStanceFootToLeftFoot(); state2->setStanceFootToRightFoot(); state3->setStanceFootToRightFoot(); // Set global desired pelvis angle state0->setDesiredPelvisGlobalAngleOnSagital(0.0_deg); state1->setDesiredPelvisGlobalAngleOnSagital(0.0_deg); state2->setDesiredPelvisGlobalAngleOnSagital(0.0_deg); state3->setDesiredPelvisGlobalAngleOnSagital(0.0_deg); state0->setDesiredPelvisGlobalAngleOnCoronal(0.0_deg); state1->setDesiredPelvisGlobalAngleOnCoronal(0.0_deg); state2->setDesiredPelvisGlobalAngleOnCoronal(0.0_deg); state3->setDesiredPelvisGlobalAngleOnCoronal(0.0_deg); // Set desired joint position //-- State 0 //---- pelvis state0->setDesiredJointPosition( "back_bky", -pelvis); // angle b/w pelvis and torso //---- swing leg state0->setDesiredJointPosition("r_leg_hpy", -swh02); // right hip state0->setDesiredJointPosition("r_leg_kny", -swk02); // right knee state0->setDesiredJointPosition("r_leg_aky", -swa02); // right ankle //---- stance leg state0->setDesiredJointPosition("l_leg_kny", -stk02); // left knee state0->setDesiredJointPosition("l_leg_aky", -sta02); // left ankle //---- arm state0->setDesiredJointPosition("l_arm_shy", -20.00_deg); // left arm state0->setDesiredJointPosition("r_arm_shy", +10.00_deg); // right arm state0->setDesiredJointPosition("l_arm_shx", -80.00_deg); // left arm state0->setDesiredJointPosition("r_arm_shx", +80.00_deg); // right arm //---- feedback gain for hip joints state0->setFeedbackCoronalCOMDistance( mCoronalLeftHip, -cd); // coronal left hip state0->setFeedbackCoronalCOMVelocity( mCoronalLeftHip, -cv); // coronal left hip state0->setFeedbackCoronalCOMDistance( mCoronalRightHip, -cd); // coronal right hip state0->setFeedbackCoronalCOMVelocity( mCoronalRightHip, -cv); // coronal right hip state0->setFeedbackSagitalCOMDistance( mSagitalLeftHip, -cd); // sagital left hip state0->setFeedbackSagitalCOMVelocity( mSagitalLeftHip, -cv); // sagital left hip state0->setFeedbackSagitalCOMDistance( mSagitalRightHip, -cd); // sagital right hip state0->setFeedbackSagitalCOMVelocity( mSagitalRightHip, -cv); // sagital right hip //-- State 1 //---- pelvis state1->setDesiredJointPosition( "back_bky", -pelvis); // angle b/w pelvis and torso //---- swing leg state1->setDesiredJointPosition("l_leg_hpy", -swh13); // left hip state1->setDesiredJointPosition("l_leg_kny", -swk13); // left knee state1->setDesiredJointPosition("l_leg_aky", -swa13); // left ankle //---- stance leg state1->setDesiredJointPosition("r_leg_kny", -stk13); // right knee state1->setDesiredJointPosition("r_leg_aky", -sta13); // right ankle //---- arm state1->setDesiredJointPosition("l_arm_shy", +10.00_deg); // left arm state1->setDesiredJointPosition("r_arm_shy", -20.00_deg); // right arm state1->setDesiredJointPosition("l_arm_shx", -80.00_deg); // left arm state1->setDesiredJointPosition("r_arm_shx", +80.00_deg); // right arm //---- feedback gain for hip joints state1->setFeedbackCoronalCOMDistance( mCoronalLeftHip, -cd); // coronal left hip state1->setFeedbackCoronalCOMVelocity( mCoronalLeftHip, -cv); // coronal left hip state1->setFeedbackCoronalCOMDistance( mCoronalRightHip, -cd); // coronal right hip state1->setFeedbackCoronalCOMVelocity( mCoronalRightHip, -cv); // coronal right hip state1->setFeedbackSagitalCOMDistance( mSagitalLeftHip, -cd); // sagital left hip state1->setFeedbackSagitalCOMVelocity( mSagitalLeftHip, -cv); // sagital left hip state1->setFeedbackSagitalCOMDistance( mSagitalRightHip, -cd); // sagital right hip state1->setFeedbackSagitalCOMVelocity( mSagitalRightHip, -cv); // sagital right hip //-- State 2 //---- pelvis state2->setDesiredJointPosition( "back_bky", -pelvis); // angle b/w pelvis and torso //---- swing leg state2->setDesiredJointPosition("l_leg_hpy", -swh02); // left hip state2->setDesiredJointPosition("l_leg_kny", -swk02); // left knee state2->setDesiredJointPosition("l_leg_aky", -swa02); // left ankle //---- stance leg state2->setDesiredJointPosition("r_leg_kny", -stk02); // right knee state2->setDesiredJointPosition("r_leg_aky", -sta02); // right ankle //---- arm state2->setDesiredJointPosition("l_arm_shy", +10.00_deg); // left arm state2->setDesiredJointPosition("r_arm_shy", -20.00_deg); // right arm state2->setDesiredJointPosition("l_arm_shx", -80.00_deg); // left arm state2->setDesiredJointPosition("r_arm_shx", +80.00_deg); // right arm //---- feedback gain for hip joints state2->setFeedbackCoronalCOMDistance( mCoronalLeftHip, -cd); // coronal left hip state2->setFeedbackCoronalCOMVelocity( mCoronalLeftHip, -cv); // coronal left hip state2->setFeedbackCoronalCOMDistance( mCoronalRightHip, -cd); // coronal right hip state2->setFeedbackCoronalCOMVelocity( mCoronalRightHip, -cv); // coronal right hip state2->setFeedbackSagitalCOMDistance( mSagitalLeftHip, -cd); // sagital left hip state2->setFeedbackSagitalCOMVelocity( mSagitalLeftHip, -cv); // sagital left hip state2->setFeedbackSagitalCOMDistance( mSagitalRightHip, -cd); // sagital right hip state2->setFeedbackSagitalCOMVelocity( mSagitalRightHip, -cv); // sagital right hip //-- State 3 //---- pelvis state3->setDesiredJointPosition( "back_bky", -pelvis); // angle b/w pelvis and torso //---- swing leg state3->setDesiredJointPosition("r_leg_hpy", -swh13); // right hip state3->setDesiredJointPosition("r_leg_kny", -swk13); // right knee state3->setDesiredJointPosition("r_leg_aky", -swa13); // right ankle //---- stance leg state3->setDesiredJointPosition("l_leg_kny", -stk13); // left knee state3->setDesiredJointPosition("l_leg_aky", -sta13); // left ankle //---- arm state3->setDesiredJointPosition("l_arm_shy", -20.00_deg); // left arm state3->setDesiredJointPosition("r_arm_shy", +10.00_deg); // right arm state3->setDesiredJointPosition("l_arm_shx", -80.00_deg); // left arm state3->setDesiredJointPosition("r_arm_shx", +80.00_deg); // right arm //---- feedback gain for hip joints state3->setFeedbackCoronalCOMDistance( mCoronalLeftHip, -cd); // coronal left hip state3->setFeedbackCoronalCOMVelocity( mCoronalLeftHip, -cv); // coronal left hip state3->setFeedbackCoronalCOMDistance( mCoronalRightHip, -cd); // coronal right hip state3->setFeedbackCoronalCOMVelocity( mCoronalRightHip, -cv); // coronal right hip state3->setFeedbackSagitalCOMDistance( mSagitalLeftHip, -cd); // sagital left hip state3->setFeedbackSagitalCOMVelocity( mSagitalLeftHip, -cv); // sagital left hip state3->setFeedbackSagitalCOMDistance( mSagitalRightHip, -cd); // sagital right hip state3->setFeedbackSagitalCOMVelocity( mSagitalRightHip, -cv); // sagital right hip sm->addState(state0); sm->addState(state1); sm->addState(state2); sm->addState(state3); sm->setInitialState(state1); return sm; }