My function is a vector of dimension 6, and on using Newton Raphson method, the solution usually converges to the nearest root. However, I know that my function has multiple roots (it's an Inverse Kinematics problem) and I would like to impose certain constraints on the roots.
For example, if the roots of the function represent the joint angles of a robot's leg, then I would like to specify that certain joints always have positive joint angles. I would thus like to always obtain a positive root, irrespective of my initial guess.
I would also like to specify that the function (a 6D vector) I am using is NOT A POLYNOMIAL function, but involves the addition and multiplication of simple trigonometric functions, that are directly function of the joint angles.
For more clarity, this is what my function looks like:
h_1-6 are the 6 elements of the function vector. The q[i] terms are the angles on which this function depends. The R*_*** terms are basically the products of some sin/cos functions of the elements of q and the dpt[][] terms are just some constants.
So essentially, I need to solve h = f(q) = 0 and the Newton-Raphson equation is:
q(N+1) = q(N) - (inv(J(q(N)) x h(q(N))) . . . . where J is the Jacobian matrix and N represents the iteration number.
q thus represents the roots of the function and I would like to impose constraints individually on the elements of q, e.g. q1 > 0, -1.4 < q[2] < 1.4 etc..
I was wondering if it would be possible to perhaps find all the roots of the function and then pick the most suitable one, or keep searching for roots (I'm really not sure how) until I find one that meets the criteria. I found some articles about how to find all the roots to a POLYNOMIAL function, but then my function is different.
The convergence of the Newton method depends on its first iteration point, so you can try to change this initial point in order to try to get different solutions.
More specifically, you can do a grid search in the region where you expect to find the solution you want, taking as a initial value of the Newton method each of the points in the grid. This can be quite convenient if you already know in which region the solution you are looking for is.