I am trying to implement inverse kinematics solver using BFGS as stated in the paper Xia2017.
In the test experiment, i created 4 objects in 3-dimensional space: Node, Node1, Node2, Node3. Each Node is a child of the one before it, so they represent a serial link. Each node has its local coordinate frame and orientation of each frame is the same as global frame's orientation (initial x,y,z rotations are 0): I am controlling the system by changing x,y,z (roll, putch, yaw) rotations of each local frame. I am certain that the values I get by these transformations are correct as I tested computations versus commercial software.
My goal is to develop a solver which will take as input, some new position of the chain (4 cubes) by having global positions of all nodes and to find the angles of rotation of each node in the controlled chain (i have the desired state as global position vector and im trying to align all nodes by rotation frames) by minimizing some distance measure (sum of squared distances or average distance). As stated in the paper, this can be achieved very qucikly by optimizing cost function with BFGS (second order newtonian technique). I am using python's
scipy.optimize.minimize implementation of BFGS which as stated works well if Jacobian is provided. By approximating Jacobian with Autograd python library and running the optimizer, my line search procedure in the first iteration of descent fails as if jacobian is not properly computed and every function value in gradient direction has higher value than the initial guess.
I am wondering what is the main cause of such failure of line search? I tried changing starting point and position desired vector many times, with line search failing all the time. Even if the gradient was poorly approximated, there should be some successful search from time to time am i right? (I assume this is true for low dimensions but I fail to understand line search behaviour in more dimensions). I would appreciate good reference on this topic. Also i am open for suggestion on some other methods I could use, Gauss-newton or something similar. Time is not the main issue, I am more concerned with precision.
Xia2017 doi: 10.1007/s00371-017-1400-y