From e9896b2b30f51602c11a4fd634e3351325f6ad94 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Tue, 23 Jun 2015 00:20:54 -0400 Subject: [PATCH 1/4] added error warning and assert when a name cannot be found in a NameManager --- dart/common/detail/NameManager.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/dart/common/detail/NameManager.h b/dart/common/detail/NameManager.h index 164b274bdd4be..a59e70eeb0f74 100644 --- a/dart/common/detail/NameManager.h +++ b/dart/common/detail/NameManager.h @@ -242,7 +242,13 @@ T NameManager::getObject(const std::string& _name) const if (result != mMap.end()) return result->second; else + { + dterr << "[NameManager::getObject] (" << mManagerName + << ") Could not find an object with the name [" << _name << "]\n"; + assert(false); + return nullptr; + } } //============================================================================== From 771dfa8baa969982e671e3cda3870c2c1111072e Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Tue, 23 Jun 2015 00:57:59 -0400 Subject: [PATCH 2/4] throwing assertion causes trouble for parser, so it's removed --- dart/common/detail/NameManager.h | 6 ------ 1 file changed, 6 deletions(-) diff --git a/dart/common/detail/NameManager.h b/dart/common/detail/NameManager.h index a59e70eeb0f74..164b274bdd4be 100644 --- a/dart/common/detail/NameManager.h +++ b/dart/common/detail/NameManager.h @@ -242,13 +242,7 @@ T NameManager::getObject(const std::string& _name) const if (result != mMap.end()) return result->second; else - { - dterr << "[NameManager::getObject] (" << mManagerName - << ") Could not find an object with the name [" << _name << "]\n"; - assert(false); - return nullptr; - } } //============================================================================== From b8677a30421db2203c4e557da197721c5bbba545 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Tue, 23 Jun 2015 02:28:47 -0400 Subject: [PATCH 3/4] improved indexing for bipedStand and atlasSimbicon --- apps/atlasSimbicon/Controller.cpp | 385 +++++++++++++++--------------- apps/atlasSimbicon/Controller.h | 12 + apps/atlasSimbicon/State.cpp | 125 +++------- apps/atlasSimbicon/State.h | 30 +-- apps/bipedStand/Controller.cpp | 82 ++----- apps/bipedStand/Controller.h | 4 +- apps/bipedStand/Main.cpp | 28 +-- 7 files changed, 290 insertions(+), 376 deletions(-) diff --git a/apps/atlasSimbicon/Controller.cpp b/apps/atlasSimbicon/Controller.cpp index e8ece408cc355..740376a5bdcb9 100644 --- a/apps/atlasSimbicon/Controller.cpp +++ b/apps/atlasSimbicon/Controller.cpp @@ -61,6 +61,11 @@ Controller::Controller(SkeletonPtr _atlasRobot, mWeldJointConstraintLeftFoot(nullptr), mWeldJointConstraintRightFoot(nullptr) { + mCoronalLeftHip = mAtlasRobot->getDof("l_leg_hpx")->getIndexInSkeleton(); + mCoronalRightHip = mAtlasRobot->getDof("r_leg_hpx")->getIndexInSkeleton(); + mSagitalLeftHip = mAtlasRobot->getDof("l_leg_hpy")->getIndexInSkeleton(); + mSagitalRightHip = mAtlasRobot->getDof("r_leg_hpy")->getIndexInSkeleton(); + _buildStateMachines(); _setJointDamping(); @@ -338,16 +343,18 @@ StateMachine* Controller::_createStandingStateMachine() standingState0->setNextState(standingState0); - standingState0->setDesiredJointPosition( 9, DART_RADIAN * 15.00); // angle b/w pelvis and torso - standingState0->setDesiredJointPosition(13, DART_RADIAN * -10.00); - standingState0->setDesiredJointPosition(14, DART_RADIAN * -10.00); - standingState0->setDesiredJointPosition(17, DART_RADIAN * 30.00); // left knee - standingState0->setDesiredJointPosition(18, DART_RADIAN * 30.00); // right knee - standingState0->setDesiredJointPosition(21, DART_RADIAN * -16.80); // left ankle - standingState0->setDesiredJointPosition(22, DART_RADIAN * -16.80); // right ankle + standingState0->setDesiredJointPosition("back_bky", DART_RADIAN * 15.00); // angle b/w pelvis and torso + standingState0->setDesiredJointPosition("l_leg_hpy", DART_RADIAN * -10.00); + standingState0->setDesiredJointPosition("r_leg_hpy", DART_RADIAN * -10.00); + standingState0->setDesiredJointPosition("l_leg_kny", DART_RADIAN * 30.00); // left knee + standingState0->setDesiredJointPosition("r_leg_kny", DART_RADIAN * 30.00); // right knee + standingState0->setDesiredJointPosition("l_leg_aky", DART_RADIAN * -16.80); // left ankle + standingState0->setDesiredJointPosition("r_leg_aky", DART_RADIAN * -16.80); // right ankle + + standingState0->setDesiredJointPosition("l_arm_shx", DART_RADIAN * -90.0); // right ankle + standingState0->setDesiredJointPosition("r_arm_shx", DART_RADIAN * +90.0); // right ankle + - standingState0->setDesiredJointPosition(19, DART_RADIAN * -90.0); // right ankle - standingState0->setDesiredJointPosition(20, DART_RADIAN * +90.0); // right ankle standing->addState(standingState0); @@ -417,103 +424,103 @@ StateMachine* Controller::_createWalkingInPlaceStateMachine() // Set desired joint position //-- State 0 //---- pelvis - state0->setDesiredJointPosition( 9, -pelvis); // angle b/w pelvis and torso + state0->setDesiredJointPosition("back_bky", -pelvis); // angle b/w pelvis and torso //---- swing leg - state0->setDesiredJointPosition(14, -swh02); // right hip - state0->setDesiredJointPosition(18, -swk02); // right knee - state0->setDesiredJointPosition(22, -swa02); // right ankle + 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(17, -stk02); // left knee - state0->setDesiredJointPosition(21, -sta02); // left ankle + state0->setDesiredJointPosition("l_leg_kny", -stk02); // left knee + state0->setDesiredJointPosition("l_leg_aky", -sta02); // left ankle //---- arm - state0->setDesiredJointPosition(15, DART_RADIAN * -20.00); // left arm - state0->setDesiredJointPosition(16, DART_RADIAN * +10.00); // right arm - state0->setDesiredJointPosition(19, DART_RADIAN * -80.00); // left arm - state0->setDesiredJointPosition(20, DART_RADIAN * +80.00); // right arm + state0->setDesiredJointPosition("l_arm_shy", DART_RADIAN * -20.00); // left arm + state0->setDesiredJointPosition("r_arm_shy", DART_RADIAN * +10.00); // right arm + state0->setDesiredJointPosition("l_arm_shx", DART_RADIAN * -80.00); // left arm + state0->setDesiredJointPosition("r_arm_shx", DART_RADIAN * +80.00); // right arm //---- feedback gain for hip joints - state0->setFeedbackCoronalCOMDistance(10, -cd); // coronal left hip - state0->setFeedbackCoronalCOMVelocity(10, -cv); // coronal left hip - state0->setFeedbackCoronalCOMDistance(11, -cd); // coronal right hip - state0->setFeedbackCoronalCOMVelocity(11, -cv); // coronal right hip - state0->setFeedbackSagitalCOMDistance(13, -cd); // sagital left hip - state0->setFeedbackSagitalCOMVelocity(13, -cv); // sagital left hip - state0->setFeedbackSagitalCOMDistance(14, -cd); // sagital right hip - state0->setFeedbackSagitalCOMVelocity(14, -cv); // sagital right hip + 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( 9, -pelvis); // angle b/w pelvis and torso + state1->setDesiredJointPosition("back_bky", -pelvis); // angle b/w pelvis and torso //---- swing leg - state1->setDesiredJointPosition(13, -swh13); // left hip - state1->setDesiredJointPosition(17, -swk13); // left knee - state1->setDesiredJointPosition(21, -swa13); // left ankle + 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(18, -stk13); // right knee - state1->setDesiredJointPosition(22, -sta13); // right ankle + state1->setDesiredJointPosition("r_leg_kny", -stk13); // right knee + state1->setDesiredJointPosition("r_leg_aky", -sta13); // right ankle //---- arm - state1->setDesiredJointPosition(15, DART_RADIAN * +10.00); // left arm - state1->setDesiredJointPosition(16, DART_RADIAN * -20.00); // right arm - state1->setDesiredJointPosition(19, DART_RADIAN * -80.00); // left arm - state1->setDesiredJointPosition(20, DART_RADIAN * +80.00); // right arm + state1->setDesiredJointPosition("l_arm_shy", DART_RADIAN * +10.00); // left arm + state1->setDesiredJointPosition("r_arm_shy", DART_RADIAN * -20.00); // right arm + state1->setDesiredJointPosition("l_arm_shx", DART_RADIAN * -80.00); // left arm + state1->setDesiredJointPosition("r_arm_shx", DART_RADIAN * +80.00); // right arm //---- feedback gain for hip joints - state1->setFeedbackCoronalCOMDistance(10, -cd); // coronal left hip - state1->setFeedbackCoronalCOMVelocity(10, -cv); // coronal left hip - state1->setFeedbackCoronalCOMDistance(11, -cd); // coronal right hip - state1->setFeedbackCoronalCOMVelocity(11, -cv); // coronal right hip - state1->setFeedbackSagitalCOMDistance(13, -cd); // sagital left hip - state1->setFeedbackSagitalCOMVelocity(13, -cv); // sagital left hip - state1->setFeedbackSagitalCOMDistance(14, -cd); // sagital right hip - state1->setFeedbackSagitalCOMVelocity(14, -cv); // sagital right hip + 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( 9, -pelvis); // angle b/w pelvis and torso + state2->setDesiredJointPosition("back_bky", -pelvis); // angle b/w pelvis and torso //---- swing leg - state2->setDesiredJointPosition(13, -swh02); // left hip - state2->setDesiredJointPosition(17, -swk02); // left knee - state2->setDesiredJointPosition(21, -swa02); // left ankle + 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(18, -stk02); // right knee - state2->setDesiredJointPosition(22, -sta02); // right ankle + state2->setDesiredJointPosition("r_leg_kny", -stk02); // right knee + state2->setDesiredJointPosition("r_leg_aky", -sta02); // right ankle //---- arm - state2->setDesiredJointPosition(15, DART_RADIAN * +10.00); // left arm - state2->setDesiredJointPosition(16, DART_RADIAN * -20.00); // right arm - state2->setDesiredJointPosition(19, DART_RADIAN * -80.00); // left arm - state2->setDesiredJointPosition(20, DART_RADIAN * +80.00); // right arm + state2->setDesiredJointPosition("l_arm_shy", DART_RADIAN * +10.00); // left arm + state2->setDesiredJointPosition("r_arm_shy", DART_RADIAN * -20.00); // right arm + state2->setDesiredJointPosition("l_arm_shx", DART_RADIAN * -80.00); // left arm + state2->setDesiredJointPosition("r_arm_shx", DART_RADIAN * +80.00); // right arm //---- feedback gain for hip joints - state2->setFeedbackCoronalCOMDistance(10, -cd); // coronal left hip - state2->setFeedbackCoronalCOMVelocity(10, -cv); // coronal left hip - state2->setFeedbackCoronalCOMDistance(11, -cd); // coronal right hip - state2->setFeedbackCoronalCOMVelocity(11, -cv); // coronal right hip - state2->setFeedbackSagitalCOMDistance(13, -cd); // sagital left hip - state2->setFeedbackSagitalCOMVelocity(13, -cv); // sagital left hip - state2->setFeedbackSagitalCOMDistance(14, -cd); // sagital right hip - state2->setFeedbackSagitalCOMVelocity(14, -cv); // sagital right hip + 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( 9, -pelvis); // angle b/w pelvis and torso + state3->setDesiredJointPosition("back_bky", -pelvis); // angle b/w pelvis and torso //---- swing leg - state3->setDesiredJointPosition(14, -swh13); // right hip - state3->setDesiredJointPosition(18, -swk13); // right knee - state3->setDesiredJointPosition(22, -swa13); // right ankle + 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(17, -stk13); // left knee - state3->setDesiredJointPosition(21, -sta13); // left ankle + state3->setDesiredJointPosition("l_leg_kny", -stk13); // left knee + state3->setDesiredJointPosition("l_leg_aky", -sta13); // left ankle //---- arm - state3->setDesiredJointPosition(15, DART_RADIAN * -20.00); // left arm - state3->setDesiredJointPosition(16, DART_RADIAN * +10.00); // right arm - state3->setDesiredJointPosition(19, DART_RADIAN * -80.00); // left arm - state3->setDesiredJointPosition(20, DART_RADIAN * +80.00); // right arm + state3->setDesiredJointPosition("l_arm_shy", DART_RADIAN * -20.00); // left arm + state3->setDesiredJointPosition("r_arm_shy", DART_RADIAN * +10.00); // right arm + state3->setDesiredJointPosition("l_arm_shx", DART_RADIAN * -80.00); // left arm + state3->setDesiredJointPosition("r_arm_shx", DART_RADIAN * +80.00); // right arm //---- feedback gain for hip joints - state3->setFeedbackCoronalCOMDistance(10, -cd); // coronal left hip - state3->setFeedbackCoronalCOMVelocity(10, -cv); // coronal left hip - state3->setFeedbackCoronalCOMDistance(11, -cd); // coronal right hip - state3->setFeedbackCoronalCOMVelocity(11, -cv); // coronal right hip - state3->setFeedbackSagitalCOMDistance(13, -cd); // sagital left hip - state3->setFeedbackSagitalCOMVelocity(13, -cv); // sagital left hip - state3->setFeedbackSagitalCOMDistance(14, -cd); // sagital right hip - state3->setFeedbackSagitalCOMVelocity(14, -cv); // sagital right hip + 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); @@ -586,103 +593,103 @@ StateMachine* Controller::_createWalkingStateMachine() // Set desired joint position //-- State 0 //---- pelvis - state0->setDesiredJointPosition( 9, -pelvis); // angle b/w pelvis and torso + state0->setDesiredJointPosition("back_bky", -pelvis); // angle b/w pelvis and torso //---- swing leg - state0->setDesiredJointPosition(14, -swh02); // right hip - state0->setDesiredJointPosition(18, -swk02); // right knee - state0->setDesiredJointPosition(22, -swa02); // right ankle + 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(17, -stk02); // left knee - state0->setDesiredJointPosition(21, -sta02); // left ankle + state0->setDesiredJointPosition("l_leg_kny", -stk02); // left knee + state0->setDesiredJointPosition("l_leg_aky", -sta02); // left ankle //---- arm - state0->setDesiredJointPosition(15, DART_RADIAN * -20.00); // left arm - state0->setDesiredJointPosition(16, DART_RADIAN * +10.00); // right arm - state0->setDesiredJointPosition(19, DART_RADIAN * -80.00); // left arm - state0->setDesiredJointPosition(20, DART_RADIAN * +80.00); // right arm + state0->setDesiredJointPosition("l_arm_shy", DART_RADIAN * -20.00); // left arm + state0->setDesiredJointPosition("r_arm_shy", DART_RADIAN * +10.00); // right arm + state0->setDesiredJointPosition("l_arm_shx", DART_RADIAN * -80.00); // left arm + state0->setDesiredJointPosition("r_arm_shx", DART_RADIAN * +80.00); // right arm //---- feedback gain for hip joints - state0->setFeedbackCoronalCOMDistance(10, -cd); // coronal left hip - state0->setFeedbackCoronalCOMVelocity(10, -cv); // coronal left hip - state0->setFeedbackCoronalCOMDistance(11, -cd); // coronal right hip - state0->setFeedbackCoronalCOMVelocity(11, -cv); // coronal right hip - state0->setFeedbackSagitalCOMDistance(13, -cd); // sagital left hip - state0->setFeedbackSagitalCOMVelocity(13, -cv); // sagital left hip - state0->setFeedbackSagitalCOMDistance(14, -cd); // sagital right hip - state0->setFeedbackSagitalCOMVelocity(14, -cv); // sagital right hip + 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( 9, -pelvis); // angle b/w pelvis and torso + state1->setDesiredJointPosition("back_bky", -pelvis); // angle b/w pelvis and torso //---- swing leg - state1->setDesiredJointPosition(13, -swh13); // left hip - state1->setDesiredJointPosition(17, -swk13); // left knee - state1->setDesiredJointPosition(21, -swa13); // left ankle + 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(18, -stk13); // right knee - state1->setDesiredJointPosition(22, -sta13); // right ankle + state1->setDesiredJointPosition("r_leg_kny", -stk13); // right knee + state1->setDesiredJointPosition("r_leg_aky", -sta13); // right ankle //---- arm - state1->setDesiredJointPosition(15, DART_RADIAN * +10.00); // left arm - state1->setDesiredJointPosition(16, DART_RADIAN * -20.00); // right arm - state1->setDesiredJointPosition(19, DART_RADIAN * -80.00); // left arm - state1->setDesiredJointPosition(20, DART_RADIAN * +80.00); // right arm + state1->setDesiredJointPosition("l_arm_shy", DART_RADIAN * +10.00); // left arm + state1->setDesiredJointPosition("r_arm_shy", DART_RADIAN * -20.00); // right arm + state1->setDesiredJointPosition("l_arm_shx", DART_RADIAN * -80.00); // left arm + state1->setDesiredJointPosition("r_arm_shx", DART_RADIAN * +80.00); // right arm //---- feedback gain for hip joints - state1->setFeedbackCoronalCOMDistance(10, -cd); // coronal left hip - state1->setFeedbackCoronalCOMVelocity(10, -cv); // coronal left hip - state1->setFeedbackCoronalCOMDistance(11, -cd); // coronal right hip - state1->setFeedbackCoronalCOMVelocity(11, -cv); // coronal right hip - state1->setFeedbackSagitalCOMDistance(13, -cd); // sagital left hip - state1->setFeedbackSagitalCOMVelocity(13, -cv); // sagital left hip - state1->setFeedbackSagitalCOMDistance(14, -cd); // sagital right hip - state1->setFeedbackSagitalCOMVelocity(14, -cv); // sagital right hip + 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( 9, -pelvis); // angle b/w pelvis and torso + state2->setDesiredJointPosition("back_bky", -pelvis); // angle b/w pelvis and torso //---- swing leg - state2->setDesiredJointPosition(13, -swh02); // left hip - state2->setDesiredJointPosition(17, -swk02); // left knee - state2->setDesiredJointPosition(21, -swa02); // left ankle + 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(18, -stk02); // right knee - state2->setDesiredJointPosition(22, -sta02); // right ankle + state2->setDesiredJointPosition("r_leg_kny", -stk02); // right knee + state2->setDesiredJointPosition("r_leg_aky", -sta02); // right ankle //---- arm - state2->setDesiredJointPosition(15, DART_RADIAN * +10.00); // left arm - state2->setDesiredJointPosition(16, DART_RADIAN * -20.00); // right arm - state2->setDesiredJointPosition(19, DART_RADIAN * -80.00); // left arm - state2->setDesiredJointPosition(20, DART_RADIAN * +80.00); // right arm + state2->setDesiredJointPosition("l_arm_shy", DART_RADIAN * +10.00); // left arm + state2->setDesiredJointPosition("r_arm_shy", DART_RADIAN * -20.00); // right arm + state2->setDesiredJointPosition("l_arm_shx", DART_RADIAN * -80.00); // left arm + state2->setDesiredJointPosition("r_arm_shx", DART_RADIAN * +80.00); // right arm //---- feedback gain for hip joints - state2->setFeedbackCoronalCOMDistance(10, -cd); // coronal left hip - state2->setFeedbackCoronalCOMVelocity(10, -cv); // coronal left hip - state2->setFeedbackCoronalCOMDistance(11, -cd); // coronal right hip - state2->setFeedbackCoronalCOMVelocity(11, -cv); // coronal right hip - state2->setFeedbackSagitalCOMDistance(13, -cd); // sagital left hip - state2->setFeedbackSagitalCOMVelocity(13, -cv); // sagital left hip - state2->setFeedbackSagitalCOMDistance(14, -cd); // sagital right hip - state2->setFeedbackSagitalCOMVelocity(14, -cv); // sagital right hip + 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( 9, -pelvis); // angle b/w pelvis and torso + state3->setDesiredJointPosition("back_bky", -pelvis); // angle b/w pelvis and torso //---- swing leg - state3->setDesiredJointPosition(14, -swh13); // right hip - state3->setDesiredJointPosition(18, -swk13); // right knee - state3->setDesiredJointPosition(22, -swa13); // right ankle + 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(17, -stk13); // left knee - state3->setDesiredJointPosition(21, -sta13); // left ankle + state3->setDesiredJointPosition("l_leg_kny", -stk13); // left knee + state3->setDesiredJointPosition("l_leg_aky", -sta13); // left ankle //---- arm - state3->setDesiredJointPosition(15, DART_RADIAN * -20.00); // left arm - state3->setDesiredJointPosition(16, DART_RADIAN * +10.00); // right arm - state3->setDesiredJointPosition(19, DART_RADIAN * -80.00); // left arm - state3->setDesiredJointPosition(20, DART_RADIAN * +80.00); // right arm + state3->setDesiredJointPosition("l_arm_shy", DART_RADIAN * -20.00); // left arm + state3->setDesiredJointPosition("r_arm_shy", DART_RADIAN * +10.00); // right arm + state3->setDesiredJointPosition("l_arm_shx", DART_RADIAN * -80.00); // left arm + state3->setDesiredJointPosition("r_arm_shx", DART_RADIAN * +80.00); // right arm //---- feedback gain for hip joints - state3->setFeedbackCoronalCOMDistance(10, -cd); // coronal left hip - state3->setFeedbackCoronalCOMVelocity(10, -cv); // coronal left hip - state3->setFeedbackCoronalCOMDistance(11, -cd); // coronal right hip - state3->setFeedbackCoronalCOMVelocity(11, -cv); // coronal right hip - state3->setFeedbackSagitalCOMDistance(13, -cd); // sagital left hip - state3->setFeedbackSagitalCOMVelocity(13, -cv); // sagital left hip - state3->setFeedbackSagitalCOMDistance(14, -cd); // sagital right hip - state3->setFeedbackSagitalCOMVelocity(14, -cv); // sagital right hip + 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); @@ -735,61 +742,61 @@ StateMachine* Controller::_createRunningStateMachine() // Set desired joint position //-- State 0 //---- pelvis - state0->setDesiredJointPosition( 9, -pelvis); // angle b/w pelvis and torso + state0->setDesiredJointPosition("back_bky", -pelvis); // angle b/w pelvis and torso //---- swing leg - state0->setDesiredJointPosition(14, -swh01); // right hip - state0->setDesiredJointPosition(18, -swk01); // right knee - state0->setDesiredJointPosition(22, -swa01); // right ankle + 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(17, -stk01); // left knee - state0->setDesiredJointPosition(21, -sta01); // left ankle + state0->setDesiredJointPosition("l_leg_kny", -stk01); // left knee + state0->setDesiredJointPosition("l_leg_aky", -sta01); // left ankle //---- arm - state0->setDesiredJointPosition(15, DART_RADIAN * -45.00); // left arm - state0->setDesiredJointPosition(16, DART_RADIAN * +15.00); // right arm - state0->setDesiredJointPosition(19, DART_RADIAN * -80.00); // left arm - state0->setDesiredJointPosition(20, DART_RADIAN * +80.00); // right arm + state0->setDesiredJointPosition("l_arm_shy", DART_RADIAN * -45.00); // left arm + state0->setDesiredJointPosition("r_arm_shy", DART_RADIAN * +15.00); // right arm + state0->setDesiredJointPosition("l_arm_shx", DART_RADIAN * -80.00); // left arm + state0->setDesiredJointPosition("r_arm_shx", DART_RADIAN * +80.00); // 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(10, -cd); // coronal left hip - state0->setFeedbackCoronalCOMVelocity(10, -cv); // coronal left hip - state0->setFeedbackCoronalCOMDistance(11, -cd); // coronal right hip - state0->setFeedbackCoronalCOMVelocity(11, -cv); // coronal right hip - state0->setFeedbackSagitalCOMDistance(13, -cd); // sagital left hip - state0->setFeedbackSagitalCOMVelocity(13, -cv); // sagital left hip - state0->setFeedbackSagitalCOMDistance(14, -cd); // sagital right hip - state0->setFeedbackSagitalCOMVelocity(14, -cv); // sagital right hip + 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( 9, -pelvis); // angle b/w pelvis and torso + state1->setDesiredJointPosition("back_bky", -pelvis); // angle b/w pelvis and torso //---- swing leg - state1->setDesiredJointPosition(13, -swh01); // left hip - state1->setDesiredJointPosition(17, -swk01); // left knee - state1->setDesiredJointPosition(21, -swa01); // left ankle + 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(18, -stk01); // right knee - state1->setDesiredJointPosition(22, -sta01); // right ankle + state1->setDesiredJointPosition("r_leg_kny", -stk01); // right knee + state1->setDesiredJointPosition("r_leg_aky", -sta01); // right ankle //---- arm - state1->setDesiredJointPosition(15, DART_RADIAN * +15.00); // left arm - state1->setDesiredJointPosition(16, DART_RADIAN * -45.00); // right arm - state1->setDesiredJointPosition(19, DART_RADIAN * -80.00); // left arm - state1->setDesiredJointPosition(20, DART_RADIAN * +80.00); // right arm + state1->setDesiredJointPosition("l_arm_shy", DART_RADIAN * +15.00); // left arm + state1->setDesiredJointPosition("r_arm_shy", DART_RADIAN * -45.00); // right arm + state1->setDesiredJointPosition("l_arm_shx", DART_RADIAN * -80.00); // left arm + state1->setDesiredJointPosition("r_arm_shx", DART_RADIAN * +80.00); // 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(10, -cd); // coronal left hip - state1->setFeedbackCoronalCOMVelocity(10, -cv); // coronal left hip - state1->setFeedbackCoronalCOMDistance(11, -cd); // coronal right hip - state1->setFeedbackCoronalCOMVelocity(11, -cv); // coronal right hip - state1->setFeedbackSagitalCOMDistance(13, -cd); // sagital left hip - state1->setFeedbackSagitalCOMVelocity(13, -cv); // sagital left hip - state1->setFeedbackSagitalCOMDistance(14, -cd); // sagital right hip - state1->setFeedbackSagitalCOMVelocity(14, -cv); // sagital right hip + 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); diff --git a/apps/atlasSimbicon/Controller.h b/apps/atlasSimbicon/Controller.h index 170cf45d329c1..7e3c080e6df17 100644 --- a/apps/atlasSimbicon/Controller.h +++ b/apps/atlasSimbicon/Controller.h @@ -124,6 +124,18 @@ class Controller /// \brief Flag for right foot harnessing bool mRightFootHarnessOn; + /// \brief Index for coronal left hip + size_t mCoronalLeftHip; + + /// \brief Index for coronal right hip + size_t mCoronalRightHip; + + /// \brief Index for sagital left hip + size_t mSagitalLeftHip; + + /// \brief Index for sagital right hip + size_t mSagitalRightHip; + private: /// \brief Check if this controller contains _stateMachine bool _containStateMachine(const StateMachine* _stateMachine) const; diff --git a/apps/atlasSimbicon/State.cpp b/apps/atlasSimbicon/State.cpp index 006f552bf1f0d..ae47c1eb0fdf2 100644 --- a/apps/atlasSimbicon/State.cpp +++ b/apps/atlasSimbicon/State.cpp @@ -50,43 +50,6 @@ using namespace Eigen; using namespace dart::dynamics; using namespace dart::constraint; -//============================================================================== -void createIndexing(std::vector&) -{ - // Do nothing -} - -//============================================================================== -template -void createIndexing(std::vector& _indexing, size_t _name, - Args... args) -{ - _indexing.push_back(_name); - createIndexing(_indexing, args...); -} - -//============================================================================== -static std::vector createIndexing() -{ - // This app was made with dof indices hardcoded, but some internal DART - // development have changed the underlying indexing for the robot, so this - // map converts the old indexing into the new indexing as a temporary (or - // maybe permanent) workaround. - - std::vector indexing; - // 0 1 2 3 4 5 6 - createIndexing(indexing, 0, 1, 2, 3, 4, 5, 6, - // 7 8 9 10 11 12 13 - 14, 26, 7, 15, 27, 8, 16, - // 14 15 16 17 18 19 20 - 28, 9, 21, 17, 29, 10, 22, - // 21 22 23 24 25 26 27 - 18, 30, 11, 23, 19, 31, 12, - // 28 29 30 31 32 - 24, 13, 25, 20, 32); - return indexing; -} - //============================================================================== State::State(SkeletonPtr _skeleton, const std::string& _name) : mName(_name), @@ -99,8 +62,7 @@ State::State(SkeletonPtr _skeleton, const std::string& _name) mDesiredGlobalSwingLegAngleOnSagital(0.0), mDesiredGlobalSwingLegAngleOnCoronal(0.0), mDesiredGlobalPelvisAngleOnSagital(0.0), - mDesiredGlobalPelvisAngleOnCoronal(0.0), - mDofMapping(createIndexing()) + mDesiredGlobalPelvisAngleOnCoronal(0.0) { int dof = mSkeleton->getNumDofs(); @@ -120,8 +82,6 @@ State::State(SkeletonPtr _skeleton, const std::string& _name) mKd[i] = ATLAS_DEFAULT_KD; } - _buildJointMap(); - mPelvis = mSkeleton->getBodyNode("pelvis"); mLeftFoot = mSkeleton->getBodyNode("l_foot"); mRightFoot = mSkeleton->getBodyNode("r_foot"); @@ -135,6 +95,11 @@ State::State(SkeletonPtr _skeleton, const std::string& _name) assert(mLeftThigh != nullptr); assert(mRightThigh != nullptr); // assert(mStanceFoot != nullptr); + + mCoronalLeftHip = mSkeleton->getDof("l_leg_hpx")->getIndexInSkeleton(); // 10 + mCoronalRightHip = mSkeleton->getDof("r_leg_hpx")->getIndexInSkeleton(); // 11 + mSagitalLeftHip = mSkeleton->getDof("l_leg_hpy")->getIndexInSkeleton(); // 13 + mSagitalRightHip = mSkeleton->getDof("r_leg_hpy")->getIndexInSkeleton(); // 14 } //============================================================================== @@ -183,8 +148,8 @@ void State::computeControlForce(double _timestep) assert(mNextState != nullptr && "Next state should be set."); int dof = mSkeleton->getNumDofs(); - VectorXd q = mSkeleton->getPositions(mDofMapping); - VectorXd dq = mSkeleton->getVelocities(mDofMapping); + VectorXd q = mSkeleton->getPositions(); + VectorXd dq = mSkeleton->getVelocities(); // Compute relative joint angles from desired global angles of the pelvis and // the swing leg @@ -235,7 +200,7 @@ void State::computeControlForce(double _timestep) _updateTorqueForStanceLeg(); // Apply control torque to the skeleton - mSkeleton->setForces(mDofMapping, mTorque); + mSkeleton->setForces(mTorque); mElapsedTime += _timestep; mFrame++; @@ -465,15 +430,6 @@ double State::getCoronalRightLegAngle() const return -angle; } -//============================================================================== -void State::_buildJointMap() -{ - mJointMap.clear(); - - for (size_t i = 0; i < mSkeleton->getNumBodyNodes(); ++i) - mJointMap[mSkeleton->getJoint(i)->getName()] = i; -} - //============================================================================== Eigen::Vector3d State::_getJointPosition(BodyNode* _bodyNode) const { @@ -504,10 +460,10 @@ void State::_updateTorqueForStanceLeg() double tauTorsoSagital = -5000.0 * (pelvisSagitalAngle + mDesiredGlobalPelvisAngleOnSagital) - 1.0 * (0); - mTorque[13] = tauTorsoSagital - mTorque[14]; + mTorque[mSagitalLeftHip] = tauTorsoSagital - mTorque[mSagitalRightHip]; -// cout << "Torque[13] : " << mTorque[13] << endl; -// cout << "Torque[14] : " << mTorque[14] << endl; +// cout << "Torque[mSagitalLeftHip] : " << mTorque[mSagitalLeftHip] << endl; +// cout << "Torque[mSagitalRightHip] : " << mTorque[mSagitalRightHip] << endl; // cout << "tauTorsoSagital: " << tauTorsoSagital << endl; // cout << endl; @@ -516,10 +472,10 @@ void State::_updateTorqueForStanceLeg() double tauTorsoCoronal = -5000.0 * (pelvisCoronalAngle - mDesiredGlobalPelvisAngleOnCoronal) - 1.0 * (0); - mTorque[10] = -tauTorsoCoronal - mTorque[11]; + mTorque[mCoronalLeftHip] = -tauTorsoCoronal - mTorque[mCoronalRightHip]; -// cout << "Torque[10] : " << mTorque[10] << endl; -// cout << "Torque[11] : " << mTorque[11] << endl; +// cout << "Torque[mCoronalLeftHip] : " << mTorque[mCoronalLeftHip] << endl; +// cout << "Torque[mCoronalRightHip] : " << mTorque[mCoronalRightHip] << endl; // cout << "tauTorsoCoronal: " << tauTorsoCoronal << endl; // cout << endl; @@ -537,10 +493,10 @@ void State::_updateTorqueForStanceLeg() double tauTorsoSagital = -5000.0 * (pelvisSagitalAngle + mDesiredGlobalPelvisAngleOnSagital) - 1.0 * (0); - mTorque[14] = tauTorsoSagital - mTorque[13]; + mTorque[mSagitalRightHip] = tauTorsoSagital - mTorque[mSagitalLeftHip]; -// cout << "Torque[13] : " << mTorque[13] << endl; -// cout << "Torque[14] : " << mTorque[14] << endl; +// cout << "Torque[mSagitalLeftHip] : " << mTorque[mSagitalLeftHip] << endl; +// cout << "Torque[mSagitalRightHip] : " << mTorque[mSagitalRightHip] << endl; // cout << "tauTorsoSagital: " << tauTorsoSagital << endl; // cout << endl; @@ -549,10 +505,10 @@ void State::_updateTorqueForStanceLeg() double tauTorsoCoronal = -5000.0 * (pelvisCoronalAngle - mDesiredGlobalPelvisAngleOnCoronal) - 1.0 * (0); - mTorque[11] = -tauTorsoCoronal - mTorque[10]; + mTorque[mCoronalRightHip] = -tauTorsoCoronal - mTorque[mCoronalLeftHip]; - // cout << "Torque[10] : " << mTorque[10] << endl; - // cout << "Torque[11] : " << mTorque[11] << endl; + // cout << "Torque[mCoronalLeftHip] : " << mTorque[mCoronalLeftHip] << endl; + // cout << "Torque[mCoronalRightHip] : " << mTorque[mCoronalRightHip] << endl; // cout << "tauTorsoCoronal: " << tauTorsoCoronal << endl; // cout << endl; } @@ -574,24 +530,11 @@ double State::getElapsedTime() const return mElapsedTime; } -//============================================================================== -void State::setDesiredJointPosition(int _idx, double _val) -{ - assert(0 <= _idx && _idx <= mDesiredJointPositions.size() - && "Invalid joint index."); - - mDesiredJointPositions[_idx] = _val; -} - //============================================================================== void State::setDesiredJointPosition(const string& _jointName, double _val) { - // TODO(JS) - NOT_YET(State::setDesiredJointPosition()); - - assert(mSkeleton->getJoint(_jointName) != nullptr); - - mDesiredJointPositions[mJointMap[_jointName]] = _val; + size_t index = mSkeleton->getDof(_jointName)->getIndexInSkeleton(); + mDesiredJointPositions[index] = _val; } //============================================================================== @@ -703,35 +646,35 @@ double State::getDerivativeGain(int _idx) const //} //============================================================================== -void State::setFeedbackSagitalCOMDistance(int _idx, double _val) +void State::setFeedbackSagitalCOMDistance(size_t _index, double _val) { - assert(0 <= _idx && _idx <= mSagitalCd.size() && "Invalid index."); + assert(0 <= _index && _index <= mSagitalCd.size() && "Invalid index."); - mSagitalCd[_idx] = _val; + mSagitalCd[_index] = _val; } //============================================================================== -void State::setFeedbackSagitalCOMVelocity(int _idx, double _val) +void State::setFeedbackSagitalCOMVelocity(size_t _index, double _val) { - assert(0 <= _idx && _idx <= mSagitalCv.size() && "Invalid index."); + assert(0 <= _index && _index <= mSagitalCv.size() && "Invalid index."); - mSagitalCv[_idx] = _val; + mSagitalCv[_index] = _val; } //============================================================================== -void State::setFeedbackCoronalCOMDistance(int _idx, double _val) +void State::setFeedbackCoronalCOMDistance(size_t _index, double _val) { assert(0 <= _idx && _idx <= mCoronalCd.size() && "Invalid index."); - mCoronalCd[_idx] = _val; + mCoronalCd[_index] = _val; } //============================================================================== -void State::setFeedbackCoronalCOMVelocity(int _idx, double _val) +void State::setFeedbackCoronalCOMVelocity(size_t _index, double _val) { - assert(0 <= _idx && _idx <= mCoronalCv.size() && "Invalid index."); + assert(0 <= _index && _index <= mCoronalCv.size() && "Invalid index."); - mCoronalCv[_idx] = _val; + mCoronalCv[_index] = _val; } //============================================================================== diff --git a/apps/atlasSimbicon/State.h b/apps/atlasSimbicon/State.h index be74f54590a84..6accbd0e72510 100644 --- a/apps/atlasSimbicon/State.h +++ b/apps/atlasSimbicon/State.h @@ -95,9 +95,6 @@ class State double getElapsedTime() const; //----------------------- Setting Desired Position --------------------------- - /// \brief Set desired joint position whose index is _idx - void setDesiredJointPosition(int _idx, double _val); - /// \brief Set desired joint position whose name is _jointName void setDesiredJointPosition(const std::string& _jointName, double _val); @@ -144,16 +141,16 @@ class State // double getDerivativeGain(const std::string& _jointName) const; /// \brief Set balance feedback gain parameter for sagital com distance - void setFeedbackSagitalCOMDistance(int _idx, double _val); + void setFeedbackSagitalCOMDistance(size_t _index, double _val); /// \brief Set balance feedback gain parameter for sagital com velocity - void setFeedbackSagitalCOMVelocity(int _idx, double _val); + void setFeedbackSagitalCOMVelocity(size_t _index, double _val); /// \brief Set balance feedback gain parameter for sagital com distance - void setFeedbackCoronalCOMDistance(int _idx, double _val); + void setFeedbackCoronalCOMDistance(size_t _index, double _val); /// \brief Set balance feedback gain parameter for sagital com velocity - void setFeedbackCoronalCOMVelocity(int _idx, double _val); + void setFeedbackCoronalCOMVelocity(size_t _index, double _val); /// \brief Set stance foot to left foot void setStanceFootToLeftFoot(); @@ -294,14 +291,7 @@ class State /// \brief Joint map std::map mJointMap; - /// \brief Maps the dofs from the indexing expected by the controller to the - /// Skeleton's actual indexing - std::vector mDofMapping; - private: - /// \brief Build joint map - void _buildJointMap(); - /// \brief Get the parent joint's position of _bodyNode Eigen::Vector3d _getJointPosition(dart::dynamics::BodyNode* _bodyNode) const; @@ -330,6 +320,18 @@ class State /// \brief Right foot body node dart::dynamics::BodyNode* mRightFoot; + /// \brief Index for coronal left hip + size_t mCoronalLeftHip; + + /// \brief Index for coronal right hip + size_t mCoronalRightHip; + + /// \brief Index for sagital left hip + size_t mSagitalLeftHip; + + /// \brief Index for sagital right hip + size_t mSagitalRightHip; + /// \brief Desired joint positions with balance feedback Eigen::VectorXd mDesiredJointPositionsBalance; }; diff --git a/apps/bipedStand/Controller.cpp b/apps/bipedStand/Controller.cpp index b44651aff16b2..64cafa603f5ea 100644 --- a/apps/bipedStand/Controller.cpp +++ b/apps/bipedStand/Controller.cpp @@ -37,59 +37,17 @@ #include "apps/bipedStand/Controller.h" -void createIndexing(std::vector&) -{ - // Do nothing -} - -template -void createIndexing(std::vector& _indexing, size_t _name, - Args... args) -{ - _indexing.push_back(_name); - createIndexing(_indexing, args...); -} - -static std::vector createIndexing() -{ - // This app was made with dof indices hardcoded, but some internal DART - // development have changed the underlying indexing for the robot, so this - // map converts the old indexing into the new indexing as a temporary (or - // maybe permanent) workaround. - - std::vector indexing; - // 0 1 2 3 4 5 6 - createIndexing(indexing, 0, 1, 2, 3, 4, 5, 6, - // 7 8 9 10 11 12 13 - 7, 8, 13, 14, 15, 20, 21, - // 14 15 16 17 18 19 20 - 9, 16, 22, 10, 11, 17, 18, - // 21 22 23 24 25 26 27 - 23, 24, 25, 31, 12, 19, 26, - // 28 29 30 31 32 33 34 - 27, 28, 32, 33, 34, 29, 35, - // 35 36 - 30, 36); - return indexing; -} - -static std::vector getDofs( - const dart::dynamics::SkeletonPtr& _skel) -{ - std::vector dofs; - const std::vector& indexing = createIndexing(); - dofs.reserve(indexing.size()); - - for(size_t index : indexing) - dofs.push_back(_skel->getDof(index)); - - return dofs; -} - Controller::Controller(dart::dynamics::SkeletonPtr _skel, double _t) { - mSkel = dart::dynamics::Group::create("Group", getDofs(_skel)); + mSkel = _skel; mLeftHeel = _skel->getBodyNode("h_heel_left"); + + mLeftFoot[0] = _skel->getDof("j_heel_left_1")->getIndexInSkeleton(); + mLeftFoot[1] = _skel->getDof("j_toe_left")->getIndexInSkeleton(); + + mRightFoot[0] = _skel->getDof("j_heel_right_1")->getIndexInSkeleton(); + mRightFoot[1] = _skel->getDof("j_toe_right")->getIndexInSkeleton(); + mTimestep = _t; mFrame = 0; int nDof = mSkel->getNumDofs(); @@ -99,7 +57,7 @@ Controller::Controller(dart::dynamics::SkeletonPtr _skel, mTorques.resize(nDof); mTorques.setZero(); - resetDesiredDofs(); + mDesiredDofs = mSkel->getPositions(); // using SPD results in simple Kp coefficients for (int i = 0; i < 6; i++) { @@ -129,10 +87,6 @@ void Controller::setDesiredDof(int _index, double _val) { mDesiredDofs[_index] = _val; } -void Controller::resetDesiredDofs() { - mDesiredDofs = mSkel->getPositions(); -} - void Controller::computeTorques() { Eigen::VectorXd _dof = mSkel->getPositions(); Eigen::VectorXd _dofVel = mSkel->getVelocities(); @@ -152,25 +106,25 @@ void Controller::computeTorques() { Eigen::Vector3d com = mSkel->getCOM(); Eigen::Vector3d cop = mLeftHeel->getTransform() * Eigen::Vector3d(0.05, 0, 0); - Eigen::Vector2d diff(com[0] - cop[0], com[2] - cop[2]); + double offset = com[0] - cop[0]; if (offset < 0.1 && offset > 0.0) { double k1 = 200.0; double k2 = 100.0; double kd = 10.0; - mTorques[17] += -k1 * offset + kd * (mPreOffset - offset); - mTorques[25] += -k2 * offset + kd * (mPreOffset - offset); - mTorques[19] += -k1 * offset + kd * (mPreOffset - offset); - mTorques[26] += -k2 * offset + kd * (mPreOffset - offset); + mTorques[mLeftFoot[0]] += -k1 * offset + kd * (mPreOffset - offset); + mTorques[mLeftFoot[1]] += -k2 * offset + kd * (mPreOffset - offset); + mTorques[mRightFoot[0]] += -k1 * offset + kd * (mPreOffset - offset); + mTorques[mRightFoot[1]] += -k2 * offset + kd * (mPreOffset - offset); mPreOffset = offset; } else if (offset > -0.2 && offset < -0.05) { double k1 = 2000.0; double k2 = 100.0; double kd = 100.0; - mTorques[17] += -k1 * offset + kd * (mPreOffset - offset); - mTorques[25] += -k2 * offset + kd * (mPreOffset - offset); - mTorques[19] += -k1 * offset + kd * (mPreOffset - offset); - mTorques[26] += -k2 * offset + kd * (mPreOffset - offset); + mTorques[mLeftFoot[0]] += -k1 * offset + kd * (mPreOffset - offset); + mTorques[mLeftFoot[1]] += -k2 * offset + kd * (mPreOffset - offset); + mTorques[mRightFoot[0]] += -k1 * offset + kd * (mPreOffset - offset); + mTorques[mRightFoot[1]] += -k2 * offset + kd * (mPreOffset - offset); mPreOffset = offset; } diff --git a/apps/bipedStand/Controller.h b/apps/bipedStand/Controller.h index 887c0f85c4cab..7ab9a1c0f7f67 100644 --- a/apps/bipedStand/Controller.h +++ b/apps/bipedStand/Controller.h @@ -53,7 +53,6 @@ class Controller { Eigen::VectorXd getTorques(); double getTorque(int _index); void setDesiredDof(int _index, double _val); - void resetDesiredDofs(); void computeTorques(); dart::dynamics::MetaSkeletonPtr getSkel(); Eigen::VectorXd getDesiredDofs(); @@ -67,9 +66,12 @@ class Controller { Eigen::VectorXd mDesiredDofs; Eigen::MatrixXd mKp; Eigen::MatrixXd mKd; + size_t mLeftFoot[2]; + size_t mRightFoot[2]; int mFrame; double mTimestep; double mPreOffset; + /// \brief SPD utilizes the current info about contact forces }; diff --git a/apps/bipedStand/Main.cpp b/apps/bipedStand/Main.cpp index 0e604364e4b80..adac6224000b0 100644 --- a/apps/bipedStand/Main.cpp +++ b/apps/bipedStand/Main.cpp @@ -52,25 +52,19 @@ int main(int argc, char* argv[]) { Eigen::Vector3d gravity(0.0, -9.81, 0.0); myWorld->setGravity(gravity); - // create controller - Controller* myController = new Controller(myWorld->getSkeleton(1), - myWorld->getTimeStep()); - dart::dynamics::MetaSkeletonPtr group = myController->getSkel(); + dart::dynamics::SkeletonPtr biped = myWorld->getSkeleton("fullbody1"); - std::vector genCoordIds; - genCoordIds.push_back(1); - genCoordIds.push_back(6); // left hip - genCoordIds.push_back(14); // left knee - genCoordIds.push_back(17); // left ankle - genCoordIds.push_back(9); // right hip - genCoordIds.push_back(15); // right knee - genCoordIds.push_back(19); // right ankle - genCoordIds.push_back(13); // lower back - Eigen::VectorXd initConfig(8); - initConfig << -0.2, 0.15, -0.4, 0.25, 0.15, -0.4, 0.25, 0.0; - group->setPositions(genCoordIds, initConfig); + biped->getDof("j_pelvis_rot_y")->setPosition( -0.20); + biped->getDof("j_thigh_left_z")->setPosition( 0.15); + biped->getDof("j_shin_left")->setPosition( -0.40); + biped->getDof("j_heel_left_1")->setPosition( 0.25); + biped->getDof("j_thigh_right_z")->setPosition( 0.15); + biped->getDof("j_shin_right")->setPosition( -0.40); + biped->getDof("j_heel_right_1")->setPosition( 0.25); + biped->getDof("j_abdomen_2")->setPosition( 0.00); - myController->resetDesiredDofs(); + // create controller + Controller* myController = new Controller(biped, myWorld->getTimeStep()); // create a window and link it to the world MyWindow window; From 433b970df0282e87171d10354ef4da9694c3f523 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 25 Jun 2015 05:18:00 +0900 Subject: [PATCH 4/4] Update changelog.md --- Changelog.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/Changelog.md b/Changelog.md index 33a422b94d033..e4b461cab7040 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,3 +1,8 @@ +### Version 5.0.1 (2015-XX-XX) + +1. Improved app indexing for bipedStand and atlasSimbicon + * [Pull request #417](https://github.com/dartsim/dart/pull/417) + ### Version 5.0.0 (2015-06-15) 1. Fixed aligned memory allocation with Eigen objects