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; }