Skip to content

Commit

Permalink
Merge pull request #417 from dartsim/fix_app_indexing
Browse files Browse the repository at this point in the history 
Improved app indexing for bipedStand and atlasSimbicon
  • Loading branch information
@jslee02
jslee02 committed Jun 25, 2015
2 parents 9245949 + 433b970 commit 65f8ff1
Show file tree
Hide file tree
Showing 8 changed files with 295 additions and 376 deletions.
5 changes: 5 additions & 0 deletions Changelog.md
View file
Original file line number Diff line number Diff line change
@@ -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
Expand Down
385 changes: 196 additions & 189 deletions apps/atlasSimbicon/Controller.cpp
View file

Large diffs are not rendered by default.

12 changes: 12 additions & 0 deletions apps/atlasSimbicon/Controller.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
125 changes: 34 additions & 91 deletions apps/atlasSimbicon/State.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,43 +50,6 @@ using namespace Eigen;
using namespace dart::dynamics;
using namespace dart::constraint;

//==============================================================================
void createIndexing(std::vector<size_t>&)
{
// Do nothing
}

//==============================================================================
template <typename ...Args>
void createIndexing(std::vector<size_t>& _indexing, size_t _name,
Args... args)
{
_indexing.push_back(_name);
createIndexing(_indexing, args...);
}

//==============================================================================
static std::vector<size_t> 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<size_t> 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),
Expand All @@ -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();

Expand All @@ -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");
Expand All @@ -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
}

//==============================================================================
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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++;
Expand Down Expand Up @@ -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
{
Expand Down Expand Up @@ -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;

Expand All @@ -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;

Expand All @@ -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;

Expand All @@ -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;
}
Expand All @@ -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;
}

//==============================================================================
Expand Down Expand Up @@ -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;
}

//==============================================================================
Expand Down
30 changes: 16 additions & 14 deletions apps/atlasSimbicon/State.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -294,14 +291,7 @@ class State
/// \brief Joint map
std::map<const std::string, int> mJointMap;

/// \brief Maps the dofs from the indexing expected by the controller to the
/// Skeleton's actual indexing
std::vector<size_t> 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;

Expand Down Expand Up @@ -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;
};
Expand Down
Loading

0 comments on commit 65f8ff1

Please sign in to comment.