//==============================================================================
void Controller::_setJointDamping()
{
for (std::size_t i = 1; i < mAtlasRobot->getNumBodyNodes(); ++i)
{
Joint* joint = mAtlasRobot->getJoint(i);
if (joint->getNumDofs() > 0)
{
for (std::size_t j = 0; j < joint->getNumDofs(); ++j)
joint->setDampingCoefficient(j, 80.0);
}
}
}
void check_self_consistency(SkeletonPtr skeleton)
{
for(size_t i=0; i<skeleton->getNumBodyNodes(); ++i)
{
BodyNode* bn = skeleton->getBodyNode(i);
EXPECT_TRUE(bn->getIndexInSkeleton() == i);
EXPECT_TRUE(skeleton->getBodyNode(bn->getName()) == bn);
Joint* joint = bn->getParentJoint();
EXPECT_TRUE(skeleton->getJoint(joint->getName()) == joint);
for(size_t j=0; j<joint->getNumDofs(); ++j)
{
DegreeOfFreedom* dof = joint->getDof(j);
EXPECT_TRUE(dof->getIndexInJoint() == j);
EXPECT_TRUE(skeleton->getDof(dof->getName()) == dof);
}
}
for(size_t i=0; i<skeleton->getNumDofs(); ++i)
{
DegreeOfFreedom* dof = skeleton->getDof(i);
EXPECT_TRUE(dof->getIndexInSkeleton() == i);
EXPECT_TRUE(skeleton->getDof(dof->getName()) == dof);
}
}
//==============================================================================
void Controller::printDebugInfo() const
{
std::cout << "[ATLAS Robot]" << std::endl
<< " NUM NODES : " << mAtlasRobot->getNumBodyNodes() << std::endl
<< " NUM DOF : " << mAtlasRobot->getNumDofs() << std::endl
<< " NUM JOINTS: " << mAtlasRobot->getNumBodyNodes() << std::endl;
for (std::size_t i = 0; i < mAtlasRobot->getNumBodyNodes(); ++i)
{
Joint* joint = mAtlasRobot->getJoint(i);
BodyNode* body = mAtlasRobot->getBodyNode(i);
BodyNode* parentBody = mAtlasRobot->getBodyNode(i)->getParentBodyNode();
std::cout << " Joint [" << i << "]: " << joint->getName() << " ("
<< joint->getNumDofs() << ")" << std::endl;
if (parentBody != nullptr)
{
std::cout << " Parent body: " << parentBody->getName() << std::endl;
}
std::cout << " Child body : " << body->getName() << std::endl;
}
}
//==============================================================================
void SoftDynamicsTest::compareEquationsOfMotion(const std::string& _fileName)
{
using namespace std;
using namespace Eigen;
using namespace dart;
using namespace math;
using namespace dynamics;
using namespace simulation;
using namespace utils;
//---------------------------- Settings --------------------------------------
// Number of random state tests for each skeletons
#ifndef NDEBUG // Debug mode
size_t nRandomItr = 1;
#else
size_t nRandomItr = 1;
#endif
// Lower and upper bound of configuration for system
double lb = -1.5 * DART_PI;
double ub = 1.5 * DART_PI;
// Lower and upper bound of joint damping and stiffness
double lbD = 0.0;
double ubD = 10.0;
double lbK = 0.0;
double ubK = 10.0;
simulation::World* myWorld = NULL;
//----------------------------- Tests ----------------------------------------
// Check whether multiplication of mass matrix and its inverse is identity
// matrix.
myWorld = utils::SkelParser::readWorld(_fileName);
EXPECT_TRUE(myWorld != NULL);
for (size_t i = 0; i < myWorld->getNumSkeletons(); ++i)
{
dynamics::Skeleton* skel = myWorld->getSkeleton(i);
dynamics::Skeleton* softSkel
= dynamic_cast<dynamics::Skeleton*>(skel);
int dof = skel->getNumDofs();
// int nBodyNodes = skel->getNumBodyNodes();
int nSoftBodyNodes = 0;
if (softSkel != NULL)
nSoftBodyNodes = softSkel->getNumSoftBodyNodes();
if (dof == 0)
{
dtmsg << "Skeleton [" << skel->getName() << "] is skipped since it has "
<< "0 DOF." << endl;
continue;
}
for (size_t j = 0; j < nRandomItr; ++j)
{
// Random joint stiffness and damping coefficient
for (size_t k = 0; k < skel->getNumBodyNodes(); ++k)
{
BodyNode* body = skel->getBodyNode(k);
Joint* joint = body->getParentJoint();
int localDof = joint->getNumDofs();
for (int l = 0; l < localDof; ++l)
{
joint->setDampingCoefficient(l, random(lbD, ubD));
joint->setSpringStiffness (l, random(lbK, ubK));
double lbRP = joint->getPositionLowerLimit(l);
double ubRP = joint->getPositionUpperLimit(l);
joint->setRestPosition (l, random(lbRP, ubRP));
}
}
// Set random states
VectorXd x = skel->getState();
for (int k = 0; k < x.size(); ++k)
x[k] = random(lb, ub);
skel->setState(x);
skel->computeForwardKinematics(true, true, false);
//------------------------ Mass Matrix Test ----------------------------
// Get matrices
MatrixXd M = skel->getMassMatrix();
MatrixXd M2 = getMassMatrix(skel);
MatrixXd InvM = skel->getInvMassMatrix();
MatrixXd M_InvM = M * InvM;
MatrixXd InvM_M = InvM * M;
MatrixXd AugM = skel->getAugMassMatrix();
MatrixXd AugM2 = getAugMassMatrix(skel);
MatrixXd InvAugM = skel->getInvAugMassMatrix();
MatrixXd AugM_InvAugM = AugM * InvAugM;
MatrixXd InvAugM_AugM = InvAugM * AugM;
MatrixXd I = MatrixXd::Identity(dof, dof);
// Check if the number of generalized coordinates and dimension of mass
// matrix are same.
EXPECT_EQ(M.rows(), dof);
EXPECT_EQ(M.cols(), dof);
// Check mass matrix
EXPECT_TRUE(equals(M, M2, 1e-6));
if (!equals(M, M2, 1e-6))
{
cout << "M :" << endl << M << endl << endl;
cout << "M2:" << endl << M2 << endl << endl;
}
// Check augmented mass matrix
EXPECT_TRUE(equals(AugM, AugM2, 1e-6));
if (!equals(AugM, AugM2, 1e-6))
{
cout << "AugM :" << endl << AugM << endl << endl;
cout << "AugM2:" << endl << AugM2 << endl << endl;
}
// Check if both of (M * InvM) and (InvM * M) are identity.
EXPECT_TRUE(equals(M_InvM, I, 1e-6));
if (!equals(M_InvM, I, 1e-6))
{
cout << "InvM :" << endl << InvM << endl << endl;
}
EXPECT_TRUE(equals(InvM_M, I, 1e-6));
if (!equals(InvM_M, I, 1e-6))
{
cout << "InvM_M:" << endl << InvM_M << endl << endl;
}
// Check if both of (M * InvM) and (InvM * M) are identity.
EXPECT_TRUE(equals(AugM_InvAugM, I, 1e-6));
if (!equals(AugM_InvAugM, I, 1e-6))
{
cout << "InvAugM :" << endl << InvAugM << endl << endl;
cout << "InvAugM2 :" << endl << AugM.inverse() << endl << endl;
cout << "AugM_InvAugM :" << endl << AugM_InvAugM << endl << endl;
}
EXPECT_TRUE(equals(InvAugM_AugM, I, 1e-6));
if (!equals(InvAugM_AugM, I, 1e-6))
{
cout << "InvAugM_AugM:" << endl << InvAugM_AugM << endl << endl;
}
//------- Coriolis Force Vector and Combined Force Vector Tests --------
// Get C1, Coriolis force vector using recursive method
VectorXd C = skel->getCoriolisForces();
VectorXd Cg = skel->getCoriolisAndGravityForces();
// Get C2, Coriolis force vector using inverse dynamics algorithm
Vector3d oldGravity = skel->getGravity();
VectorXd oldTau = skel->getForces();
VectorXd oldDdq = skel->getAccelerations();
// TODO(JS): Save external forces of body nodes
vector<double> oldKv(nSoftBodyNodes, 0.0);
vector<double> oldKe(nSoftBodyNodes, 0.0);
vector<double> oldD(nSoftBodyNodes, 0.0);
for (int k = 0; k < nSoftBodyNodes; ++k)
{
assert(softSkel != NULL);
dynamics::SoftBodyNode* sbn = softSkel->getSoftBodyNode(k);
oldKv[k] = sbn->getVertexSpringStiffness();
oldKe[k] = sbn->getEdgeSpringStiffness();
oldD[k] = sbn->getDampingCoefficient();
}
skel->resetForces();
skel->clearExternalForces();
skel->setAccelerations(VectorXd::Zero(dof));
for (int k = 0; k < nSoftBodyNodes; ++k)
{
assert(softSkel != NULL);
dynamics::SoftBodyNode* sbn = softSkel->getSoftBodyNode(k);
sbn->setVertexSpringStiffness(0.0);
sbn->setEdgeSpringStiffness(0.0);
sbn->setDampingCoefficient(0.0);
}
EXPECT_TRUE(skel->getForces() == VectorXd::Zero(dof));
EXPECT_TRUE(skel->getExternalForces() == VectorXd::Zero(dof));
EXPECT_TRUE(skel->getAccelerations() == VectorXd::Zero(dof));
skel->setGravity(Vector3d::Zero());
EXPECT_TRUE(skel->getGravity() == Vector3d::Zero());
skel->computeInverseDynamics(false, false);
VectorXd C2 = skel->getForces();
skel->setGravity(oldGravity);
EXPECT_TRUE(skel->getGravity() == oldGravity);
skel->computeInverseDynamics(false, false);
VectorXd Cg2 = skel->getForces();
EXPECT_TRUE(equals(C, C2, 1e-6));
if (!equals(C, C2, 1e-6))
{
cout << "C :" << C.transpose() << endl;
cout << "C2:" << C2.transpose() << endl;
}
EXPECT_TRUE(equals(Cg, Cg2, 1e-6));
if (!equals(Cg, Cg2, 1e-6))
{
cout << "Cg :" << Cg.transpose() << endl;
cout << "Cg2:" << Cg2.transpose() << endl;
}
skel->setForces(oldTau);
skel->setAccelerations(oldDdq);
// TODO(JS): Restore external forces of body nodes
}
}
delete myWorld;
}
// Current code only works for the left leg with only one constraint
VectorXd MyWorld::updateGradients() {
mJ.setZero();
mC.setZero();
// compute c(q)
//std::cout << "HAMYDEBUG: mConstrainedMarker = " << getMarker(mConstrainedMarker) << std::endl;
mC = getMarker(mConstrainedMarker)->getWorldPosition() - mTarget;
std::cout << "C(q) = " << mC << std::endl;
// compute J(q)
Vector4d offset;
offset << getMarker(mConstrainedMarker)->getLocalPosition(), 1; // Create a vector in homogeneous coordinates
//Setup vars
BodyNode *node = getMarker(mConstrainedMarker)->getBodyNode();
Joint *joint = node->getParentJoint();
Matrix4d worldToParent;
Matrix4d parentToJoint;
//Declare Vars
Matrix4d dR; // Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d
Matrix4d R;
Matrix4d R1;
Matrix4d R2;
Matrix4d jointToChild;
Vector4d jCol;
int colIndex;
//TODO: Might want to change this to check if root using given root fcn
//Iterate until we get to the root node
while(true) {
//std::cout << "HAMY DEBUG: Beginning new looop" << std::endl;
if(node->getParentBodyNode() == NULL) {
worldToParent = worldToParent.setIdentity();
} else {
worldToParent = node->getParentBodyNode()->getTransform().matrix();
}
parentToJoint = joint->getTransformFromParentBodyNode().matrix();
// Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d
jointToChild = joint->getTransformFromChildBodyNode().inverse().matrix();
//TODO: R1, R2, ... Rn code depending on DOFs
int nDofs = joint->getNumDofs();
//std::cout << "HAMY: nDofs=" << nDofs << std::endl;
//Can only have up to 3 DOFs on any one piece
switch(nDofs){
case 1: //std::cout << "HAMY: 1 nDOF" << std::endl;
dR = joint->getTransformDerivative(0);
jCol = worldToParent * parentToJoint * dR * jointToChild * offset;
colIndex = joint->getIndexInSkeleton(0);
mJ.col(colIndex) = jCol.head(3); // Take the first 3 elelemtns of jCol
offset = parentToJoint * joint->getTransform(0).matrix() * jointToChild * offset;
break;
case 2: //std::cout << "HAMY: 2 nDOF" << std::endl;
dR = joint->getTransformDerivative(0); // Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d
R = joint->getTransform(1).matrix();
jointToChild = joint->getTransformFromChildBodyNode().inverse().matrix();
jCol = worldToParent * parentToJoint * dR * R * jointToChild * offset;
colIndex = joint->getIndexInSkeleton(0);
mJ.col(colIndex) = jCol.head(3); // Take the first 3 elelemtns of jCol
dR = joint->getTransformDerivative(1);
R = joint->getTransform(0).matrix();
jCol = worldToParent * parentToJoint * R * dR * jointToChild * offset;
colIndex = joint->getIndexInSkeleton(1);
mJ.col(colIndex) = jCol.head(3);
offset = parentToJoint * joint->getTransform(0).matrix() * joint->getTransform(1).matrix() * jointToChild * offset; // Upd
break;
case 3: //std::cout << "HAMY: 3 nDOF" << std::endl;
dR = joint->getTransformDerivative(0); // Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d
R1 = joint->getTransform(1).matrix();
R2 = joint->getTransform(2).matrix();
jointToChild = joint->getTransformFromChildBodyNode().inverse().matrix();
jCol = worldToParent * parentToJoint * dR * R1 * R2 * jointToChild * offset;
colIndex = joint->getIndexInSkeleton(0);
mJ.col(colIndex) = jCol.head(3); // Take the first 3 elelemtns of J
R1 = joint->getTransform(0).matrix();
dR = joint->getTransformDerivative(1);
R2 = joint->getTransform(2).matrix();
jCol = worldToParent * parentToJoint * R1 * dR * R2 * jointToChild * offset;
colIndex = joint->getIndexInSkeleton(1);
mJ.col(colIndex) = jCol.head(3);
R1 = joint->getTransform(0).matrix();
R2 = joint->getTransform(1).matrix();
dR = joint->getTransformDerivative(2);
jCol = worldToParent * parentToJoint * R1 * R2 * dR * jointToChild * offset;
colIndex = joint->getIndexInSkeleton(2);
mJ.col(colIndex) = jCol.head(3);
offset = parentToJoint * joint->getTransform(0).matrix() * joint->getTransform(1).matrix() * joint->getTransform(2).matrix() * jointToChild * offset;
break;
default: //std::cout << "HAMY: Unexpected nDOF = " << nDofs << std::endl;
break;
}
if(node != mSkel->getRootBodyNode()) {
//std::cout << "HAMY DEBUG: Not root, continue loop" << std::endl;
node = node->getParentBodyNode(); // return NULL if node is the root node
joint = node->getParentJoint();
} else {
break;
}
}
// compute gradients
VectorXd gradients = 2 * mJ.transpose() * mC;
return gradients;
}
