Apply a global scale on StdCostFunc to try to normalize the number of robot.
Compute the kinematics with the starting point to compute initial force.
Update the pgdata before launching the optimization to avoid some issues with first constraint call.