double TangentSearch::find_root(double f(double x),
double f_prime(double x))
{
double f0 = f(x0);
double f_prime0 = f_prime(x0);
if ( f0 == 0 )
return x0;
if ( f_prime0 != 0 )
dx = - f0 / f_prime0;
int step = 0;
if ( verbose ) {
printHead("Tangent Search", accuracy, *os);
printStep(step, x0, dx, f0, *os);
}
do {
if ( ++step > max_steps )
break;
if ( f_prime0 == 0 ) {
cerr << " Tangent Search: f'(x0) = 0, algorithm fails!\n"
<< " f(" << x0 << ") = " << f0 << endl
<< " f'(" << x0 << ") = " << f_prime0 << endl;
break;
}
dx = - f0 / f_prime0;
x0 += dx;
f0 = f(x0);
f_prime0 = f_prime(x0);
if ( verbose )
printStep(step, x0, dx, f0, *os);
} while ( abs(dx) > accuracy && f0 != 0);
if ( step > max_steps )
printWarning(max_steps);
steps = step;
return x0;
}
/*
* Open Newton-Raphson method root solver
*
* Takes a function pointer (*f) for the function to be solved, expects
* an inital guess for the root x_0, and the error tolerance.
* Returns the solution.
*
* @param *f double(*_)(double) function pointer to the function for which we want to find a root
* @param root double with initial guess for the root location
* @param numIterations int reference to count and pass back the number of iterations required to solve this problem
* @param errLimit double providing the error tolerance
* @return double return the root solution within the bracket
*/
double rootSolvers::newton(double (*f)(double), double (*f_prime)(double),
double root, int& numIterations, double errLimit)
{
numIterations = 0;
double err = 1.0;
while ((err > errLimit) && (numIterations < 10000))
{
double r_old = root;
double slope_tangent = f_prime(r_old);
std::cout << "It: " << numIterations << "\n";
std::cout << "root: " << root << "\n";
// std::cout << "Slope at r_old: " << slope_tangent << "\n";
if (relErr(0.0, slope_tangent) < 0.000001)
{
throw std::runtime_error(
"slope_tangent == 0, Newton method failed.");
}
root = (r_old - (f(r_old) / f_prime(r_old)));
numIterations++;
if (root != 0.0)
{
err = relErr(root, r_old);
}
}
double solution_check = f(root);
if ((relErr(0.0, solution_check) > 0.01) || (numIterations == 100000))
{
throw std::runtime_error("No convergence, Newton method failed.");
}
return root;
}
double newton(double x_0, double tol, int max_iters,
int* iters_p, int* converged_p) {
double x = x_0;
double x_prev;
int iter = 0;
do {
iter++;
x_prev = x;
x = x_prev - f(x_prev)/f_prime(x_prev);
} while (fabs(x - x_prev) > tol && iter < max_iters);
if (fabs(x - x_prev) <= tol)
*converged_p = 1;
else
*converged_p = 0;
*iters_p = iter;
return x;
} /* newton algorithm */
ActLens Leg::solveIK(const Vec3f& p) {
/** Given an input foot location p, finds necessary actuator lengths to obtain this position
*/
ActLens output;
// Calculate angle around hip pivot
float swingAngle = flipped ? PI/2 + atan2f(p.y, p.x) : PI/2-atan2f(p.y, p.x);
// Unproject foot point based on swingAngle so the rest
// can be solved as flat triangles
Vec3f f_prime(sqrtf(p.x*p.x + p.y*p.y)-swingArm, 0, p.z);
// Distance from bottom of leg to foot
float D = Vec3f(0,0,hip).dist(f_prime);
// Distance from top of leg to foot
float E = f_prime.mag();
// Calculate some interior angles
float gamma = acosf((femur*femur+D*D-(tibia+cankle)*(tibia+cankle)) /
(2*femur*D));
float sigma = acosf((D*D+hip*hip-E*E) / (2*D*hip));
float alpha = f_prime.x > 0 ? (sigma - gamma) : (TWO_PI - sigma - gamma);
// Law of cosines trigonometry gives magic answers...
output.a = sqrtf(femur*femur+hip*hip-2*femur*hip*cosf(alpha));
output.b = sqrtf(femur*femur+tibia*tibia -
(tibia*((tibia+cankle)*(tibia+cankle)+femur*femur-D*D) /
(tibia+cankle)));
output.c = sqrtf(swingMount*swingMount +
swingBase*swingBase -
2*swingMount*swingBase *
cosf(swingAngle + frameAngle));
return output;
}
double SquareError::error (double output, double target) const {
return m_actfun->f_prime_from_f(output) * f_prime(output, target);
}
