bool GenericIKSolver::solve(const Eigen::Matrix4f &globalPose, CartesianSelection selection, int maxLoops)
{
jacobian->setGoal(globalPose,tcp,selection,maxErrorPositionMM,maxErrorOrientationRad);
jacobian->checkImprovements(true);
RobotConfigPtr bestConfig(new RobotConfig(rns->getRobot(),"bestConfig"));
rns->getJointValues(bestConfig);
float bestError = jacobian->getMeanErrorPosition();
// first check reachability
if (!checkReachable(globalPose))
return false;
// first run: start with current joint angles
if (trySolve())
return true;
// if here we failed
for (int i=1; i<maxLoops; i++)
{
setJointsRandom();
if (trySolve())
return true;
float currentError = jacobian->getMeanErrorPosition();
if (currentError < bestError)
{
rns->getJointValues(bestConfig);
bestError = jacobian->getMeanErrorPosition();
}
}
// set best config
rns->setJointValues(bestConfig);
return false;
}
int trySolve(char grid[9][9], Candidates gridC[9][9], char finalGrid[9][9]) {
bool completed = true;
// Try every candidate by recursively deepening into possible configurations
for (int row = 0; row < 9; ++row) {
for (int col = 0; col < 9; ++col) {
if (!isSudokuDigit(grid[row][col])) {
completed = false;
Candidates & cands = gridC[row][col];
for (int i = 0; i < 9; ++i) {
if (cands.possible[i]) {
// Make copies
char x_grid[9][9];
Candidates x_gridC[9][9];
for (int r = 0; r < 9; ++r) {
for (int c = 0; c < 9; ++c) {
x_grid[r][c] = grid[r][c];
x_gridC[r][c] = gridC[r][c];
}
}
x_grid[row][col] = i + '1';
std::list<std::pair<int, int>> pointsOfInterest;
pointsOfInterest.push_back(
std::pair<int, int>(row, col));
while (!pointsOfInterest.empty()) {
int row = pointsOfInterest.front().first;
int col = pointsOfInterest.front().second;
pointsOfInterest.pop_front();
auto r = process(row, col, x_grid, x_gridC,
pointsOfInterest);
if (-1 == r) { return -1; }
}
if (1 == trySolve(x_grid, x_gridC, finalGrid)) {
return 1;
}
}
}
}
}
}
if (completed) {
commit(grid, finalGrid);
return 1;
}
return 0;
}
int solve(char grid[9][9]) {
// Begin by constraining possibilities (some easier puzzles can be solved
// completely merely with this method)
Candidates gridC[9][9];
std::list<std::pair<int, int>> pointsOfInterest;
for (int row = 0; row < 9; ++row) {
for (int col = 0; col < 9; ++col) {
pointsOfInterest.push_back(std::pair<int, int>(row, col));
}
}
while (!pointsOfInterest.empty()) {
int row = pointsOfInterest.front().first;
int col = pointsOfInterest.front().second;
pointsOfInterest.pop_front();
process(row, col, grid, gridC, pointsOfInterest);
}
// Recursively descend and entertain possibilities from our constrained
// sample space (further constraining on copies of candidates)
trySolve(grid, gridC, grid);
return 0;
}
