Does the angular_motor work? Or is it still broken?
I feel like you will want to work with quaternions? Or maybe some PID feedback loops to manage the forces. You give it a target calculate the error (the difference from current position to target position) then apply your PID functions to calculate a force until error is minimized.
I would cast two rays, one at the target and one at the center of the arm. The joint being the origin. Use the cross product to calculate an “area of error” the goal is minimize the error by moving the two rays to the same direction with the PID formulas.