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;
}
