Eigen::VectorXd Skeleton::getPose() {
Eigen::VectorXd pose(getNumDofs());
for (int i = 0; i < getNumDofs(); i++) {
pose(i) = mDofs[i]->getValue();
}
return pose;
}
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();
}
}
//==============================================================================
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;
}
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();
}
}
}
