static void getDOFLimits(RaveRobotObject::Ptr robot, const vector<int> &indices,
Vector7d &out_lower, Vector7d &out_upper) {
vector<double> lb(7), ub(7);
robot->robot->GetDOFLimits(lb, ub, indices);
out_lower = toEigVec(lb);
out_upper = toEigVec(ub);
cout << "dof limits: " << out_lower.transpose() << " | " << out_upper.transpose() << endl;
}
/// Read file for gains
void readGains () {
// Get the gains
Vector7d* kgains [] = {&K_stand, &K_bal};
ifstream file ("/home/cerdogan/Documents/Software/project/krang/iser/data/gains-09.txt");
assert(file.is_open());
char line [1024];
for(size_t k_idx = 0; k_idx < 2; k_idx++) {
*kgains[k_idx] = Vector7d::Zero();
file.getline(line, 1024);
std::stringstream stream(line, std::stringstream::in);
size_t i = 0;
double newDouble;
while ((i < 6) && (stream >> newDouble)) (*kgains[k_idx])(i++) = newDouble;
}
cout << "K_stand: " << K_stand.transpose() << endl;
cout << "K_bal: " << K_bal.transpose() << endl;
file.close();
}
/// Can avoid collisions and joint limits
bool singleArmIK (const VectorXd& base_conf, const Matrix4d& Twee, bool rightArm, Vector7d& theta,
bool minimizeColl) {
static const bool debug = 0;
// Get the relative goal
Transform <double, 3, Affine> relGoal;
Matrix4d Twb;
bracketTransform(base_conf, Twb);
// cout << "Twb: \n" << Twb<< "\n";
getWristInShoulder(Twb, Twee, rightArm, relGoal.matrix());
// Compute the inverse kinematics if you don't want to minimize collisions
if(!minimizeColl) {
// double phi = 0.0;
double phi = theta(0);
theta = VectorXd (7);
bool result = ik(relGoal, phi, theta);
return result;
}
// Otherwise check multiple phi values
Vector7d bestTheta;
double maxMinDistSq = -16.0;
bool result = false;
for(double phi = 0.0; phi <= M_PI; phi += M_PI/16.0) {
// Check if there is an IK
Vector7d someTheta = VectorXd(7);
bool someResult = ik(relGoal, phi, someTheta);
if(debug) cout << "phi: " << phi << ", result: " << someResult << ", theta: " << someTheta.transpose() << endl;
// If there is a successful result, see if it is the best one so far
if(someResult) {
// Set the config to the left arm and get the coms of bracket, 7 links and end-effector
krang->setConfig(left_dofs, someTheta);
const char* names [7] = {"Bracket", "L2", "L3", "L4", "L5", "L6", "lFingerA"};
Eigen::Vector3d locs [7];
for(size_t i = 0; i < 7; i++) {
locs[i] = krang->getNode(names[i])->getWorldCOM();
if(debug) cout << "\t\t locs[" << i << "] (" << names[i] << "): " << locs[i].transpose() <<
", localCOM: " << krang->getNode(names[i])->getLocalCOM().transpose() << endl;
}
// Find the distance to both joint limits for each dof and determine the one that is
// closest to the limit
double minDistSq = 16.0;
for(size_t i = 0; i < 7; i++) {
for(size_t j = i+1; j < 7; j++) {
double distSq = (locs[i] - locs[j]).squaredNorm();
if(distSq < minDistSq) minDistSq = distSq;
}
}
if(debug) cout << "\tminDistSq: " << minDistSq << ", maxMinDistSq : " << maxMinDistSq << endl;
// Check if it is more than the current maximum minAngle
if(minDistSq > maxMinDistSq) {
maxMinDistSq = minDistSq;
bestTheta = someTheta;
}
result = true;
}
}
// Set the result of sampling the space
theta = bestTheta;
return result;
}
