コード例 #1
0
ファイル: Skeleton.cpp プロジェクト: Tarrasch/dart
 Eigen::VectorXd Skeleton::getPose() {
     Eigen::VectorXd pose(getNumDofs());
     for (int i = 0; i < getNumDofs(); i++) {
         pose(i) = mDofs[i]->getValue();
     }
     return pose;
 }
コード例 #2
0
ファイル: Skeleton.cpp プロジェクト: Tarrasch/dart
    void Skeleton::initSkel() {
        mRoot = mNodes[0];

        // calculate mass
        // init the dependsOnDof stucture for each bodylink
        for(int i=0; i<getNumNodes(); i++) {
            mNodes[i]->setSkel(this);
            // mNodes[i]->setDependDofMap(getNumDofs());
            mNodes[i]->setDependDofList();
            mNodes.at(i)->init();
            mMass += mNodes[i]->getMass();
        }

        mCurrPose = VectorXd::Zero(getNumDofs());

        for(int i=0; i<getNumDofs(); i++)
            mCurrPose[i] = mDofs.at(i)->getValue();
        for(int i=0; i<getNumNodes(); i++) {
            mNodes.at(i)->updateTransform();
        }
    }
コード例 #3
0
ファイル: Joint.cpp プロジェクト: jslee02/wafr2016
//==============================================================================
bool Joint::checkSanity(bool _printWarnings) const
{
  bool sane = true;
  for(std::size_t i=0; i < getNumDofs(); ++i)
  {
    if(getInitialPosition(i) < getPositionLowerLimit(i)
       || getPositionUpperLimit(i) < getInitialPosition(i))
    {
      if(_printWarnings)
      {
        dtwarn << "[Joint::checkSanity] Initial position of index " << i << " ["
               << getDofName(i) << "] in Joint [" << getName() << "] is "
               << "outside of its position limits\n"
               << " -- Initial Position: " << getInitialPosition(i) << "\n"
               << " -- Limits: [" << getPositionLowerLimit(i) << ", "
               << getPositionUpperLimit(i) << "]\n";
      }
      else
      {
        return false;
      }

      sane = false;
    }

    if(getInitialVelocity(i) < getVelocityLowerLimit(i)
       || getVelocityUpperLimit(i) < getInitialVelocity(i))
    {
      if(_printWarnings)
      {
        dtwarn << "[Joint::checkSanity] Initial velocity of index " << i << " ["
               << getDofName(i) << "] is Joint [" << getName() << "] is "
               << "outside of its velocity limits\n"
               << " -- Initial Velocity: " << getInitialVelocity(i) << "\n"
               << " -- Limits: [" << getVelocityLowerLimit(i) << ", "
               << getVelocityUpperLimit(i) << "]\n";
      }
      else
      {
        return false;
      }

      sane = false;
    }
  }

  return sane;
}
コード例 #4
0
ファイル: Skeleton.cpp プロジェクト: Tarrasch/dart
    void Skeleton::setPose(const VectorXd& state, bool bCalcTrans, bool bCalcDeriv) {
        mCurrPose = state;
        for (int i = 0; i < getNumDofs(); i++) {
            mDofs.at(i)->setValue(state[i]);
        }

        if (bCalcTrans) {
            for (int i = 0; i < getNumNodes(); i++) {
                mNodes.at(i)->updateTransform();
            }
        }

        if (bCalcDeriv) {
            for (int i = 0; i < getNumNodes(); i++) {
                mNodes.at(i)->updateFirstDerivatives();
            }
        }
    }