   Cyclic Coordinate Descent with Collision Detection
  I'm implementing a new solver for our N axis robot arms. It's working so far with some basic CCD. I'm now tryign to add collision detection so that the links do not pass through themselves or through the environment. The axis limits was easy. I just clamped the angles to their limits while iterating. The solution, if found, always satisfies this constraint. I've implemented the same for collision detection with the enviornment and it seems to be working for the simple cases i've got setup, with concave boundaries around the robot. I have a feeling the solution will break down eventually, possibly with convex boundaries within the environment itself.For the CD, if a joint increment value found during the solving process would put part of the robot through the environment, the increment is rejected and the next axis is evaluated. If no solution is found after X passes over the system, i reset the structure and try again (once more).Any suggestions on how this is typically handled?Also any suggestions on how to incorperate pose relaxation to the solving process? As in the ideal pose would be one with the simplest or straightest possible curvature. For this i've added a factor which scales the incremental adjustments to the system. This results in more iterations being used but results in smoother poses as all the joints share the work of getting to the target rather than just the major axes. Doesnt necessarily always work, but definitely results in smoother robot movement.   