Controller::Controller(dart::dynamics::Skeleton* _skel,
dart::constraint::ConstraintDynamics* _collisionHandle,
double _t) {
mSkel = _skel;
mCollisionHandle = _collisionHandle;
mTimestep = _t;
mFrame = 0;
int nDof = mSkel->getNumGenCoords();
mKp = Eigen::MatrixXd::Identity(nDof, nDof);
mKd = Eigen::MatrixXd::Identity(nDof, nDof);
mConstrForces = Eigen::VectorXd::Zero(nDof);
mTorques.resize(nDof);
mDesiredDofs.resize(nDof);
for (int i = 0; i < nDof; i++) {
mTorques[i] = 0.0;
mDesiredDofs[i] = mSkel->getGenCoord(i)->getConfig();
}
// using SPD results in simple Kp coefficients
for (int i = 0; i < 6; i++) {
mKp(i, i) = 0.0;
mKd(i, i) = 0.0;
}
for (int i = 6; i < 22; i++)
mKp(i, i) = 200.0; // lower body + lower back
for (int i = 22; i < nDof; i++)
mKp(i, i) = 20.0;
for (int i = 6; i < 22; i++)
mKd(i, i) = 100.0;
for (int i = 22; i < nDof; i++)
mKd(i, i) = 10.0;
mPreOffset = 0.0;
}
Controller::Controller(dart::dynamics::SkeletonPtr _skel, double _t)
{
mSkel = _skel;
mLeftHeel = _skel->getBodyNode("h_heel_left");
mLeftFoot[0] = _skel->getDof("j_heel_left_1")->getIndexInSkeleton();
mLeftFoot[1] = _skel->getDof("j_toe_left")->getIndexInSkeleton();
mRightFoot[0] = _skel->getDof("j_heel_right_1")->getIndexInSkeleton();
mRightFoot[1] = _skel->getDof("j_toe_right")->getIndexInSkeleton();
mTimestep = _t;
mFrame = 0;
int nDof = mSkel->getNumDofs();
mKp = Eigen::MatrixXd::Identity(nDof, nDof);
mKd = Eigen::MatrixXd::Identity(nDof, nDof);
mTorques.resize(nDof);
mTorques.setZero();
mDesiredDofs = mSkel->getPositions();
// using SPD results in simple Kp coefficients
for (int i = 0; i < 6; i++)
{
mKp(i, i) = 0.0;
mKd(i, i) = 0.0;
}
for (int i = 6; i < nDof; i++)
mKp(i, i) = 400.0;
for (int i = 6; i < nDof; i++)
mKd(i, i) = 40.0;
mPreOffset = 0.0;
}
Controller::Controller(dart::dynamics::Skeleton* _skel, dart::constraint::ConstraintSolver* _constrSolver, double _t,
dart::simulation::World* world) {
prevXSet = false;
bentAbdomen = false;
mSkel = _skel;
mConstraintSolver = _constrSolver;
mTimestep = _t;
int nDof = mSkel->getNumDofs();
mKp = Eigen::MatrixXd::Identity(nDof, nDof);
mKd = Eigen::MatrixXd::Identity(nDof, nDof);
mTorques.resize(nDof);
mDefaultPose.resize(nDof);
mDesiredDofs.resize(nDof);
// Set the barPositions
if(world->getSkeleton("bar4")){
numBars = 4;
barPositions = new Eigen::Vector3d[4];
barPositions[0] = world->getSkeleton("bar1")->getWorldCOM();
barPositions[1] = world->getSkeleton("bar2")->getWorldCOM();
barPositions[2] = world->getSkeleton("bar3")->getWorldCOM();
barPositions[3] = world->getSkeleton("bar4")->getWorldCOM();
}
else if(world->getSkeleton("bar3")){
numBars = 3;
barPositions = new Eigen::Vector3d[3];
barPositions[0] = world->getSkeleton("bar1")->getWorldCOM();
barPositions[1] = world->getSkeleton("bar2")->getWorldCOM();
barPositions[2] = world->getSkeleton("bar3")->getWorldCOM();
}
else if(world->getSkeleton("bar2")){
numBars = 2;
barPositions = new Eigen::Vector3d[2];
barPositions[0] = world->getSkeleton("bar1")->getWorldCOM();
barPositions[1] = world->getSkeleton("bar2")->getWorldCOM();
}
else{
numBars = 1;
barPositions = new Eigen::Vector3d[1];
barPositions[0] = world->getSkeleton("bar1")->getWorldCOM();
}
handDiffZ = fabs(mSkel->getBodyNode("h_hand_left")->getWorldCOM()[2]
- mSkel->getBodyNode("h_hand_right")->getWorldCOM()[2]);
std::cout << "handDiffZ = " << handDiffZ << std::endl;
for (int i = 0; i < numBars; i++){
std::cout << "Bar WORLD COM " << i << std::endl;
std::cout << barPositions[i].transpose() << std::endl;
}
// Set default pose as the initial pose when the controller is instantiated
mDefaultPose = mSkel->getPositions();
mDesiredDofs = mDefaultPose;
std::cout << "mDesiredDofs" << std::endl;
std::cout << mDesiredDofs.transpose() << std::endl;
std::cout << "numBars" << std::endl;
std::cout << numBars << std::endl;
mTorques.setZero();
// Using SPD results in simple spring coefficients
for (int i = 0; i < 6; i++) {
mKp(i, i) = 0.0;
mKd(i, i) = 0.0;
}
for (int i = 6; i < 35; i++)
mKp(i, i) = 400.0;
for (int i = 6; i < 35; i++)
mKd(i, i) = 40.0;
// Make shoulders and elbows loose
for (int i = 27; i < 35; i++) {
//mKp(i, i) = 20.0;
//mKd(i, i) = 2.0;
mKp(i, i) = 10.0;
mKd(i, i) = 1.0;
}
// Make wrists even looser
for (int i = 35; i < 39; i++) {
mKp(i, i) = 1.0;
mKd(i, i) = 0.1;
}
mPreOffset = 0.0;
mLeftHandHold = NULL;
mRightHandHold = NULL;
mFootContact = NULL;
mLeftHandContact = NULL;
mRightHandContact = NULL;
mTimer = 300;
mState = "STAND";
mArch = 0;
}
