Skip to content

Commit

Permalink
- Replace previous forward dynamics algorithms with Featherstone's ones
Browse files Browse the repository at this point in the history 
- Use dirty flags for dynamics quantities such as M, M^{-1}, C, g, Fext, J
- Calculate the dynamics qunatities using cache data from the forward dynamics steps which improves the performance
  • Loading branch information
@jslee02
jslee02 committed Nov 20, 2013
1 parent 57dbce9 commit 5070900
Show file tree
Hide file tree
Showing 25 changed files with 1,261 additions and 585 deletions.
2 changes: 1 addition & 1 deletion apps/balance/MyWindow.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@ void MyWindow::timeStepping()

mController->setConstrForces(mWorld->getConstraintHandler()->getTotalConstraintForce(1));
mController->computeTorques(mWorld->getSkeleton(1)->get_q(), mWorld->getSkeleton(1)->get_dq());
mWorld->getSkeleton(1)->setInternalForces(mController->getTorques());
mWorld->getSkeleton(1)->setInternalForceVector(mController->getTorques());

mWorld->step();

Expand Down
2 changes: 1 addition & 1 deletion apps/forwardSim/MyWindow.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@
void MyWindow::timeStepping()
{
Eigen::VectorXd damping = computeDamping();
mWorld->getSkeleton(0)->setInternalForces(damping);
mWorld->getSkeleton(0)->setInternalForceVector(damping);
mWorld->step();
}

Expand Down
8 changes: 4 additions & 4 deletions src/constraint/ClosedLoopConstraint.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,13 +42,13 @@ void ClosedLoopConstraint::updateDynamics(std::vector<Eigen::MatrixXd> & _J, Eig

void ClosedLoopConstraint::getJacobian() {
Eigen::MatrixXd JBody1 = mBody1->getWorldJacobian(mOffset1 - mBody1->getWorldTransform().translation()).bottomRows<3>();
for(int i = 0; i < mBody1->getNumDependentDofs(); i++) {
int dofIndex = mBody1->getDependentDof(i);
for(int i = 0; i < mBody1->getNumDependentGenCoords(); i++) {
int dofIndex = mBody1->getDependentGenCoord(i);
mJ1.col(dofIndex) = JBody1.col(dofIndex);
}
Eigen::MatrixXd JBody2 = mBody2->getWorldJacobian(mOffset2 - mBody2->getWorldTransform().translation()).bottomRows<3>();
for(int i = 0; i < mBody2->getNumDependentDofs(); i++) {
int dofIndex = mBody2->getDependentDof(i);
for(int i = 0; i < mBody2->getNumDependentGenCoords(); i++) {
int dofIndex = mBody2->getDependentGenCoord(i);
mJ2.col(dofIndex) = JBody2.col(dofIndex);
}
}
Expand Down
58 changes: 28 additions & 30 deletions src/constraint/ConstraintDynamics.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@ void ConstraintDynamics::computeConstraintForces() {
mLimitingDofIndex.clear();

for (int i = 0; i < mSkeletons.size(); i++) {
if (!mSkeletons[i]->isMobile())
if (!mSkeletons[i]->isMobile() || mSkeletons[i]->getNumGenCoords() == 0)
continue;

for (int j = 0; j < mSkeletons[i]->getNumBodyNodes(); j++) {
Expand Down Expand Up @@ -149,19 +149,19 @@ void ConstraintDynamics::addSkeleton(dynamics::Skeleton* _skeleton)
}

Eigen::VectorXd newConstrForce;
if (_skeleton->isMobile())
if (_skeleton->isMobile() && _skeleton->getNumGenCoords() > 0)
newConstrForce = Eigen::VectorXd::Zero(nGenCoords);
mContactForces.push_back(newConstrForce);
mTotalConstrForces.push_back(newConstrForce);

if (_skeleton->isMobile())
if (_skeleton->isMobile() && _skeleton->getNumGenCoords() > 0)
mIndices.push_back(mIndices.back() + nGenCoords);
else
mIndices.push_back(mIndices.back());

assert(mMInv.rows() == mMInv.rows() && "The mass matrix should be square.");
int N = mMInv.rows();
if (_skeleton->isMobile())
if (_skeleton->isMobile() && _skeleton->getNumGenCoords() > 0)
N += nGenCoords;
mMInv = Eigen::MatrixXd::Zero(N, N);
mTauStar = Eigen::VectorXd::Zero(N);
Expand Down Expand Up @@ -209,7 +209,7 @@ void ConstraintDynamics::removeSkeleton(dynamics::Skeleton* _skeleton)
mTotalConstrForces.pop_back();

// Update mIndices.
if (_skeleton->isMobile())
if (_skeleton->isMobile() && _skeleton->getNumGenCoords() > 0)
for (int i = iSkeleton + 1; i < mIndices.size() - 1; ++i)
mIndices[i] = mIndices[i+1] - nGenCoords;
else
Expand All @@ -219,7 +219,7 @@ void ConstraintDynamics::removeSkeleton(dynamics::Skeleton* _skeleton)

assert(mMInv.rows() == mMInv.rows() && "The mass matrix should be square.");
int N = mMInv.rows();
if (_skeleton->isMobile())
if (_skeleton->isMobile() && _skeleton->getNumGenCoords() > 0)
N -= nGenCoords;
mMInv = Eigen::MatrixXd::Zero(N, N);
mTauStar = Eigen::VectorXd::Zero(N);
Expand Down Expand Up @@ -267,7 +267,7 @@ void ConstraintDynamics::initialize() {
}
}

if (mSkeletons[i]->isMobile()) {
if (mSkeletons[i]->isMobile() && mSkeletons[i]->getNumGenCoords() > 0) {
// Immobile objets have mass of infinity
rows += skel->getMassMatrix().rows();
cols += skel->getMassMatrix().cols();
Expand All @@ -277,7 +277,7 @@ void ConstraintDynamics::initialize() {
mContactForces.resize(mSkeletons.size());
mTotalConstrForces.resize(mSkeletons.size());
for (int i = 0; i < mSkeletons.size(); i++){
if (!mSkeletons[i]->isMobile())
if (!mSkeletons[i]->isMobile() || mSkeletons[i]->getNumGenCoords() == 0)
continue;
mContactForces[i] = Eigen::VectorXd::Zero(mSkeletons[i]->getNumGenCoords());
mTotalConstrForces[i] = Eigen::VectorXd::Zero(mSkeletons[i]->getNumGenCoords());
Expand All @@ -300,7 +300,7 @@ void ConstraintDynamics::initialize() {
for (int i = 0; i < mSkeletons.size(); i++) {
dynamics::Skeleton* skel = mSkeletons[i];
int nDofs = skel->getNumGenCoords();
if (!mSkeletons[i]->isMobile())
if (!mSkeletons[i]->isMobile() || mSkeletons[i]->getNumGenCoords() == 0)
nDofs = 0;
sumNDofs += nDofs;
mIndices.push_back(sumNDofs);
Expand All @@ -319,7 +319,7 @@ void ConstraintDynamics::computeConstraintWithoutContact() {
Eigen::VectorXd lambda = mGInv * mTauHat;

for (int i = 0; i < mSkeletons.size(); i++) {
if (!mSkeletons[i]->isMobile())
if (!mSkeletons[i]->isMobile() || mSkeletons[i]->getNumGenCoords() == 0)
continue;
mTotalConstrForces[i] = mJ[i].transpose() * lambda;
}
Expand All @@ -344,7 +344,7 @@ void ConstraintDynamics::fillMatrices() {

Eigen::VectorXd tempVec = mDt * mGInv * mTauHat;
for (int i = 0; i < mSkeletons.size(); i++) {
if (!mSkeletons[i]->isMobile())
if (!mSkeletons[i]->isMobile() || mSkeletons[i]->getNumGenCoords() == 0)
continue;
tauVec.segment(mIndices[i], mSkeletons[i]->getNumGenCoords()) = mJ[i].transpose() * tempVec;
}
Expand Down Expand Up @@ -442,7 +442,7 @@ void ConstraintDynamics::fillMatricesODE() {

Eigen::VectorXd tempVec = mDt * mGInv * mTauHat;
for (int i = 0; i < mSkeletons.size(); i++) {
if (!mSkeletons[i]->isMobile())
if (!mSkeletons[i]->isMobile() || mSkeletons[i]->getNumGenCoords() == 0)
continue;
tauVec.segment(mIndices[i], mSkeletons[i]->getNumGenCoords()) = mJ[i].transpose() * tempVec;
}
Expand Down Expand Up @@ -549,7 +549,7 @@ void ConstraintDynamics::applySolution() {

Eigen::VectorXd lambda = Eigen::VectorXd::Zero(mGInv.rows());
for (int i = 0; i < mSkeletons.size(); i++) {
if (!mSkeletons[i]->isMobile())
if (!mSkeletons[i]->isMobile() || mSkeletons[i]->getNumGenCoords() == 0)
continue;
mContactForces[i] = contactForces.segment(mIndices[i], mSkeletons[i]->getNumGenCoords());

Expand Down Expand Up @@ -608,7 +608,7 @@ void ConstraintDynamics::applySolutionODE() {

Eigen::VectorXd lambda = Eigen::VectorXd::Zero(mGInv.rows());
for (int i = 0; i < mSkeletons.size(); i++) {
if (!mSkeletons[i]->isMobile())
if (!mSkeletons[i]->isMobile() || mSkeletons[i]->getNumGenCoords() == 0)
continue;
mContactForces[i] = contactForces.segment(mIndices[i], mSkeletons[i]->getNumGenCoords());

Expand All @@ -629,11 +629,10 @@ void ConstraintDynamics::applySolutionODE() {

}


void ConstraintDynamics::updateMassMat() {
int start = 0;
for (int i = 0; i < mSkeletons.size(); i++) {
if (!mSkeletons[i]->isMobile())
if (!mSkeletons[i]->isMobile() || mSkeletons[i]->getNumGenCoords() == 0)
continue;
mMInv.block(start, start, mSkeletons[i]->getNumGenCoords(), mSkeletons[i]->getNumGenCoords()) = mSkeletons[i]->getInvMassMatrix();
start += mSkeletons[i]->getNumGenCoords();
Expand All @@ -643,10 +642,10 @@ void ConstraintDynamics::updateMassMat() {
void ConstraintDynamics::updateTauStar() {
int startRow = 0;
for (int i = 0; i < mSkeletons.size(); i++) {
if (!mSkeletons[i]->isMobile())
if (!mSkeletons[i]->isMobile() || mSkeletons[i]->getNumGenCoords() == 0)
continue;

Eigen::VectorXd tau = mSkeletons[i]->getExternalForces() + mSkeletons[i]->getInternalForces() + mSkeletons[i]->getDampingForces();
Eigen::VectorXd tau = mSkeletons[i]->getExternalForceVector() + mSkeletons[i]->getInternalForceVector() + mSkeletons[i]->getDampingForceVector();
Eigen::VectorXd tauStar = (mSkeletons[i]->getMassMatrix() * mSkeletons[i]->get_dq()) - (mDt * (mSkeletons[i]->getCombinedVector() - tau));
mTauStar.block(startRow, 0, tauStar.rows(), 1) = tauStar;
startRow += tauStar.rows();
Expand All @@ -667,7 +666,7 @@ void ConstraintDynamics::updateNBMatrices() {
Eigen::MatrixXd B21 = getTangentBasisMatrix(p, N21);
Eigen::MatrixXd B12 = -B21;

if (mSkeletons[skelID1]->isMobile()) {
if (mSkeletons[skelID1]->isMobile() && mSkeletons[skelID1]->getNumGenCoords() > 0) {
int index1 = mIndices[skelID1];
int NDOF1 = c.collisionNode1->getBodyNode()->getSkeleton()->getNumGenCoords();
// Vector3d N21 = c.normal;
Expand All @@ -677,7 +676,7 @@ void ConstraintDynamics::updateNBMatrices() {
mB.block(index1, i * mNumDir, NDOF1, mNumDir).noalias() = J21t * B21;
}

if (mSkeletons[skelID2]->isMobile()) {
if (mSkeletons[skelID2]->isMobile() && mSkeletons[skelID2]->getNumGenCoords() > 0) {
int index2 = mIndices[skelID2];
int NDOF2 = c.collisionNode2->getBodyNode()->getSkeleton()->getNumGenCoords();
//Vector3d N12 = -c.normal;
Expand Down Expand Up @@ -707,7 +706,7 @@ void ConstraintDynamics::updateNBMatricesODE() {
Eigen::MatrixXd B21 = getTangentBasisMatrixODE(p, N21);
Eigen::MatrixXd B12 = -B21;

if (mSkeletons[skelID1]->isMobile()) {
if (mSkeletons[skelID1]->isMobile() && mSkeletons[skelID1]->getNumGenCoords() > 0) {
int index1 = mIndices[skelID1];
int NDOF1 = c.collisionNode1->getBodyNode()->getSkeleton()->getNumGenCoords();
// Vector3d N21 = c.normal;
Expand All @@ -717,7 +716,7 @@ void ConstraintDynamics::updateNBMatricesODE() {
mB.block(index1, i * 2, NDOF1, 2).noalias() = J21t * B21;
}

if (mSkeletons[skelID2]->isMobile()) {
if (mSkeletons[skelID2]->isMobile() && mSkeletons[skelID2]->getNumGenCoords() > 0) {
int index2 = mIndices[skelID2];
int NDOF2 = c.collisionNode2->getBodyNode()->getSkeleton()->getNumGenCoords();
//Vector3d N12 = -c.normal;
Expand All @@ -728,7 +727,6 @@ void ConstraintDynamics::updateNBMatricesODE() {
Eigen::MatrixXd J12t = getJacobian(c.collisionNode2->getBodyNode(), p);
mN.block(index2, i, NDOF2, 1).noalias() = J12t * N12;
mB.block(index2, i * 2, NDOF2, 2).noalias() = J12t * B12;

}
}
}
Expand All @@ -740,9 +738,9 @@ Eigen::MatrixXd ConstraintDynamics::getJacobian(dynamics::BodyNode* node, const
Eigen::MatrixXd JtBody
= node->getWorldJacobian(p - node->getWorldTransform().translation()).bottomRows<3>().transpose();

for(int dofIndex = 0; dofIndex < node->getNumDependentDofs(); dofIndex++)
for(int dofIndex = 0; dofIndex < node->getNumDependentGenCoords(); dofIndex++)
{
int i = node->getDependentDof(dofIndex);
int i = node->getDependentGenCoord(dofIndex);
Jt.row(i) = JtBody.row(dofIndex);
}

Expand Down Expand Up @@ -825,18 +823,18 @@ void ConstraintDynamics::updateConstraintTerms(){
// compute JMInv, GInv, Z
mGInv.triangularView<Eigen::Lower>().setZero();
for (int i = 0; i < mSkeletons.size(); i++) {
if (!mSkeletons[i]->isMobile())
if (!mSkeletons[i]->isMobile() || mSkeletons[i]->getNumGenCoords() == 0)
continue;
mJMInv[i] = mJ[i] * mSkeletons[i]->getInvMassMatrix();
mGInv.triangularView<Eigen::Lower>() += (mJMInv[i] * mJ[i].transpose());
}
mGInv = mGInv.ldlt().solve(Eigen::MatrixXd::Identity(mTotalRows, mTotalRows));
for (int i = 0; i < mSkeletons.size(); i++) {
if (!mSkeletons[i]->isMobile())
if (!mSkeletons[i]->isMobile() || mSkeletons[i]->getNumGenCoords() == 0)
continue;
mZ.block(mIndices[i], mIndices[i], mSkeletons[i]->getNumGenCoords(), mSkeletons[i]->getNumGenCoords()).triangularView<Eigen::Lower>() = mJMInv[i].transpose() * mGInv * mJMInv[i];
for (int j = 0; j < i; j++) {
if (!mSkeletons[j]->isMobile())
if (!mSkeletons[j]->isMobile() || mSkeletons[j]->getNumGenCoords() == 0)
continue;
mZ.block(mIndices[i], mIndices[j], mSkeletons[i]->getNumGenCoords(), mSkeletons[j]->getNumGenCoords()).noalias() = mJMInv[i].transpose() * mGInv * mJMInv[j];
}
Expand All @@ -847,11 +845,11 @@ void ConstraintDynamics::updateConstraintTerms(){
double kd = 50;
mTauHat.setZero();
for (int i = 0; i < mSkeletons.size(); i++) {
if (!mSkeletons[i]->isMobile())
if (!mSkeletons[i]->isMobile() || mSkeletons[i]->getNumGenCoords() == 0)
continue;
Eigen::VectorXd qDot = mSkeletons[i]->get_dq();
mTauHat.noalias() += -(mJ[i] - mPreJ[i]) / mDt * qDot;
mTauHat.noalias() -= mJMInv[i] * (mSkeletons[i]->getInternalForces() + mSkeletons[i]->getExternalForces() - mSkeletons[i]->getCombinedVector());
mTauHat.noalias() -= mJMInv[i] * (mSkeletons[i]->getInternalForceVector() + mSkeletons[i]->getExternalForceVector() - mSkeletons[i]->getCombinedVector());
}
mTauHat -= ks * mC + kd * mCDot;
}
Expand Down
4 changes: 2 additions & 2 deletions src/constraint/PointConstraint.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,8 +35,8 @@ void PointConstraint::updateDynamics(std::vector<Eigen::MatrixXd> & _J, Eigen::V

void PointConstraint::getJacobian() {
Eigen::MatrixXd JBody = mBody->getWorldJacobian(mOffset - mBody->getWorldTransform().translation()).bottomRows<3>();
for(int i = 0; i < mBody->getNumDependentDofs(); i++) {
int dofIndex = mBody->getDependentDof(i);
for(int i = 0; i < mBody->getNumDependentGenCoords(); i++) {
int dofIndex = mBody->getDependentGenCoord(i);
mJ.col(dofIndex) = JBody.col(i);
}
}
Expand Down
31 changes: 31 additions & 0 deletions src/dynamics/BallJoint.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,6 +69,8 @@ inline void BallJoint::updateTransform()
mT = mT_ParentBodyToJoint *
math::expAngular(q) *
mT_ChildBodyToJoint.inverse();

assert(math::verifyTransform(mT));
}

inline void BallJoint::updateJacobian()
Expand All @@ -90,6 +92,22 @@ inline void BallJoint::updateJacobian()
mS.col(0) = math::AdT(mT_ChildBodyToJoint, J0);
mS.col(1) = math::AdT(mT_ChildBodyToJoint, J1);
mS.col(2) = math::AdT(mT_ChildBodyToJoint, J2);

assert(!math::isNan(mS));

// Eigen::MatrixXd JTJ = mS.transpose() * mS;
// Eigen::FullPivLU<Eigen::MatrixXd> luJTJ(JTJ);
//// Eigen::FullPivLU<Eigen::MatrixXd> luS(mS);
// double det = luJTJ.determinant();
// if (det < 1e-5)
// {
// std::cout << "ill-conditioned Jacobian in joint [" << mName << "]."
// << " The determinant of the Jacobian is (" << det << ")."
// << std::endl;
// std::cout << "rank is (" << luJTJ.rank() << ")." << std::endl;
// std::cout << "det is (" << luJTJ.determinant() << ")." << std::endl;
//// std::cout << "mS: \n" << mS << std::endl;
// }
}

inline void BallJoint::updateJacobianTimeDeriv()
Expand All @@ -114,6 +132,19 @@ inline void BallJoint::updateJacobianTimeDeriv()
mdS.col(0) = math::AdT(mT_ChildBodyToJoint, dJ0);
mdS.col(1) = math::AdT(mT_ChildBodyToJoint, dJ1);
mdS.col(2) = math::AdT(mT_ChildBodyToJoint, dJ2);

assert(!math::isNan(mdS));
}

void BallJoint::clampRotation()
{
// for (int i = 0; i < 3; i++)
// {
// if( mCoordinate[i].get_q() > M_PI )
// mCoordinate[i].set_q(mCoordinate[i].get_q() - 2*M_PI);
// if( mCoordinate[i].get_q() < -M_PI )
// mCoordinate[i].set_q(mCoordinate[i].get_q() + 2*M_PI);
// }
}

} // namespace dynamics
Expand Down
3 changes: 3 additions & 0 deletions src/dynamics/BallJoint.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,6 +64,9 @@ class BallJoint : public Joint
// Documentation inherited.
virtual void updateJacobianTimeDeriv();

// Documentation inherited.
virtual void clampRotation();

protected:
/// @brief
GenCoord mCoordinate[3];
Expand Down
Loading

0 comments on commit 5070900

Please sign in to comment.