MatrixXd ConstraintDynamics::getJacobian(kinematics::BodyNode* node, const Vector3d& p) {
int nDofs = node->getSkel()->getNumDofs();
MatrixXd Jt = MatrixXd::Zero(nDofs, 3);
Vector3d invP = xformHom(node->getWorldInvTransform(), p);
for(int dofIndex = 0; dofIndex < node->getNumDependentDofs(); dofIndex++) {
int i = node->getDependentDof(dofIndex);
Jt.row(i) = xformHom(node->getDerivWorldTransform(dofIndex), invP);
}
return Jt;
}
void ClosedLoopConstraint::getJacobian() {
for(int i = 0; i < mBody1->getNumDependentDofs(); i++) {
int dofIndex = mBody1->getDependentDof(i);
VectorXd Jcol = xformHom(mBody1->getDerivWorldTransform(i), mOffset1);
mJ1.col(dofIndex) = Jcol;
}
for(int i = 0; i < mBody2->getNumDependentDofs(); i++) {
int dofIndex = mBody2->getDependentDof(i);
VectorXd Jcol = xformHom(mBody2->getDerivWorldTransform(i), mOffset2);
mJ2.col(dofIndex) = -Jcol;
}
}
void ClosedLoopConstraint::updateDynamics(std::vector<Eigen::MatrixXd> & _J, Eigen::VectorXd & _C, Eigen::VectorXd & _CDot, int _rowIndex) {
getJacobian();
_J[mSkelIndex2].block(_rowIndex, 0, 3, mBody2->getSkel()->getNumDofs()).setZero();
_J[mSkelIndex1].block(_rowIndex, 0, 3, mBody1->getSkel()->getNumDofs()) = mJ1;
_J[mSkelIndex2].block(_rowIndex, 0, 3, mBody2->getSkel()->getNumDofs()) += mJ2;
Vector3d worldP1 = xformHom(mBody1->getWorldTransform(), mOffset1);
Vector3d worldP2 = xformHom(mBody2->getWorldTransform(), mOffset2);
VectorXd qDot1 = ((SkeletonDynamics*)mBody1->getSkel())->getPoseVelocity();
VectorXd qDot2 = ((SkeletonDynamics*)mBody2->getSkel())->getPoseVelocity();
_C.segment(_rowIndex, 3) = worldP1 - worldP2;
_CDot.segment(_rowIndex, 3) = mJ1 * qDot1 + mJ2 * qDot2;
}
void PointConstraint::getJacobian() {
for(int i = 0; i < mBody->getNumDependentDofs(); i++) {
int dofIndex = mBody->getDependentDof(i);
VectorXd Jcol = xformHom(mBody->getDerivWorldTransform(i), mOffset);
mJ.col(dofIndex) = Jcol;
}
}
void PointConstraint::updateDynamics(std::vector<Eigen::MatrixXd> & _J, Eigen::VectorXd & _C, Eigen::VectorXd & _CDot, int _rowIndex) {
getJacobian();
SkeletonDynamics *skel = (SkeletonDynamics*)mBody->getSkel();
_J[mSkelIndex].block(_rowIndex, 0, 3, skel->getNumDofs()) = mJ;
Vector3d worldP = xformHom(mBody->getWorldTransform(), mOffset);
VectorXd qDot = skel->getQDotVector();
_C.segment(_rowIndex, 3) = worldP - mTarget;
_CDot.segment(_rowIndex, 3) = mJ * qDot;
}
void PositionConstraint::fillObjGrad(std::vector<double>& dG) {
VectorXd dP = evalCon();
for(int dofIndex = 0; dofIndex < mNode->getNumDependentDofs(); dofIndex++) {
int i = mNode->getDependentDof(dofIndex);
const Var* v = mVariables[i];
double w = v->mWeight;
VectorXd J = xformHom(mNode->getDerivWorldTransform(dofIndex), mOffset);
J /= w;
dG[i] += 2 * dP.dot(J);
}
}
