Mathematics desk
< July 18	<< Jun \| July \| Aug >>	July 20 >

Welcome to the Wikipedia Mathematics Reference Desk Archives
The page you are currently viewing is an archive page. While you can leave answers for any questions shown below, please ask new questions on one of the current reference desk pages.

July 19

trigonometry for a robot arm

I am trying to program a robot-arm, for which I first made equations of how angles determine the position of the tip.
Mx are the motors, segxx are the segments they control and cfmx are correction factors (when 0° is not a convenient angle).

x = cos(M0+cfm0) * (seg02 + cos(M1)*seg1 + cos(M2+cfm2-90)*seg21 + cos(M2+cfm2)*seg22)

y = sin(M0) * (seg02 + cos(M1)*seg1 + cos(M2+cfm2-90)*seg21 + cos(M2+cfm2)*seg22)

z = seg01 + sin(M1)*seg1 + cos(M2+cfm2)*seg21 + cos(M2+cfm2+90)*seg22 - seg3

But I want that in reverse, find the angles for a certain position.
From the last two equations I get this:

M0 = arcsin (y / (seg02 + cos(M1)*seg1 + cos(M2+cfm2-90)*seg21 + cos(M2+cfm2)*seg22))

M1 = arcsin ((z - seg01 - cos(M2+cfm2)*seg21 - cos(M2+cfm2+90)*seg22 + seg3) / seg1)

But how do I find M2? It is split in two parts, because segment 2 is not 'in line' with M2. DirkvdM (talk) 11:12, 19 July 2016 (UTC)[reply]
Note: I have limited it to a situation where there can only be one solution - seg3 always points down, so I left M3 out of the equation because M2+M3=constant, so I can do that calculation afterwards. DirkvdM (talk) 12:06, 19 July 2016 (UTC)[reply]

I've dealt with this problem before, and, indeed, the issue was that the equations were under-constrained. That is, there was more than one solution (locking a joint, as you did, might well over-constrain the problem, meaning there is no solution).

I ended up using a numerical methods hill-climbing approach instead. That is, I virtually moved one joint a step (starting with the first joint, that is, the one farthest from the "hand"), determined if that movement put the tip closer or farther from the target, and kept going if it was closer, or went the reverse direction otherwise. When it went from going closer in one step to farther in the next, I went on and did the same thing in the next joint, using the closest position for the first joint. I then did the same for the remaining joint(s).

I then went back to the first joint and tried moving it one step each way to see if changing it would now put the tip closer to the target. I continued to iterate until no change occurred in any joint for a full iteration.

Now this approach isn't absolutely guaranteed to be optimal for all theoretical joint geometries, but it does tend to be "good enough" for robot joints which are actually used. As far as performance goes, any delay due to calculation time is trivial compared to the time it takes to move the joints. A gantry robot, moving in X, Y, and Z directions, doesn't have all this control complexity, so is far easier to program. StuRat (talk) 13:24, 19 July 2016 (UTC)[reply]

~~Wikipedia does not have an article on that~~, but the term to search in your favorite search engine is "inverse kinematics problem". You will find, among others, chapter 3 of [1]. Tigraan^{Click here to contact me} 16:58, 19 July 2016 (UTC)[reply]

Wikipedia does have an article on inverse kinematics. -- BenRG (talk) 23:12, 19 July 2016 (UTC)[reply]

StuRat, that is pretty much the solution I came up with, except making a virtual move of all segements at the same time, based on some heuristics (I have yet to come up with :) ). And of course the first steps are big, and every time it overshoots the target, stepsize is reduced (eg halved), until the fault is smaller than the accuracy of the robot (or what is required for the task at hand).

But then I thought that surely there must be a better way. But apparently, the iteration is a normal approach, so I will use that. (I suck at trigonometry, so this makes me quite happy. :) ) I had my doubts about it, because if the microcontroller in the robot (an Arduino Due) has to do this, it might be very slow. Though speed is not really an issue. I might even pre-program most moves, because many are fixed, and the robot can then do the fine-tuning.

Tigraan, I also found that article, but I'm no mathamatician, and I just need to solve one specific problem (for now ...).

Btw, I also tried Pythagoras, but got stuck there too. Apparently, there is no solution for √(a² -b²). Well, it is √(a+b) * √(a-b), but that doesn't help. The paper uses polar coordinates (p 98), a combination of Pythagoras and trigonometry. Maybe I'll look into that later (if I am to become the company's robotics 'expert' :) ). DirkvdM (talk) 08:57, 20 July 2016 (UTC)[reply]

I think my iterative method will work better, because the arms are typically set up so that moving the first joint causes more movement in the tip than the later joints. Therefore, you want to do the rough adjustments before you do the fine tuning. Also, if you move all joints at once, you don't know which joint movements made it closer and which may have been counter-productive. To do some quick calculations, if each joint has N possible positions, and there are M joints, my method might take about MN calculations, as a worst case scenario, and half that, on average. So, we would likely be in the thousands of calculations, not millions or billions. For any modern processor, that should be trivial.

But, if you do want to optimize my method, you could add in your method of starting with large steps, then reducing the step size, on the first joint, then repeating for the 2nd joint, etc. StuRat (talk) 13:32, 20 July 2016 (UTC)[reply]

Yeah, you're probably right about doing one segment at a time. Except maybe when it is obvious that the goal is so far away that the first segement alone can't reach it. But that requires a programming effort that is probably not necessary given the speed of even a microcontroller. I just put the three equations in a loop and even when I let the program do the calculations 10 million times it takes about 8 s. That's for 11 trigonometric calculations. I thought those might take relatively long, but even then, computers have become mind-bogglingly fast. Even if the microcontroller is ten times slower, it will still not be an issue. DirkvdM (talk) 15:22, 20 July 2016 (UTC)[reply]

I don't see why doing one joint at a time won't work, even when one segment alone isn't long enough to reach. I suggest you try my approach and yours, and benchmark both. Something else you might want to add, after either method, is to just try every permutation of every joint, say within 10 steps each way, once you reach the supposedly optimal solution. This a double check. StuRat (talk) 16:20, 20 July 2016 (UTC)[reply]

I didn't say your method wouldn't work in that case, just that it might be done with fewer calculations. But that's only in such an instance, which would have to be checked first, and then it would require extra programming for a negligible advantage. So just ignore that remark.

If the stepsize halves with each iteration then after just 10 iterations you already get a precision that is 1024 times smaller then the initial step. I just realised that if that first step is 90° I don't need to check the distance to reduce step size, because the maximum range of the motors is 180°. I just need to check in which direction it should go (so if the target is overshot). And then the (theoretical!) precision of the motors of 0,18° is already reached after 9 iterations. So for 3 motors, as in this case, the 3 calculations need only be done 30 times, which is a far cry from those 10 million.

However, this isn't entirely correct. In this case there is only 1 solution. M0 rotates along the z-axis, and that is independent of the others, so the above method works for that. But M1 and M2 are interdependent. So I suppose I should do the calculations for both and then check which one brings the tip closer to the target. That shouldn't add much to the number of calculations. But even if it grows a hundredfold (which it certainly won't) that will still amount to a negligible overall calculation time.

To finish this off, the calculations have to be done for both the start and end position (unless the start position is already know, resulting from the last move). And then, to let the arm make a nice linear movement, I can calculate the speeds of the motors as the relative distances they have to move. Eg if the angle change of M1 is twice that of M2, then its speed should also be twice that of M2. Calculating the overall (linear) speed is relatively unimportant, I'd say. It just shouldn't be too high (it's a cheap robot arm, so easy does it).

About your double check, that sounds like a good idea, although I wonder if my method for M1 and M2 doesn't already cover that. At first I misunderstood, thinking it was to make sure the local optimum is also the global one. But there is only one optimum.

I don't believe we can guarantee that there is no local optimum which differs from the global optimum. Seems like just due to rounding, the step size, and how moving a joint changes more than one dimension at a time, including the locations of subsequent joints, there could be some. And there may be multiple solutions, each of which has it's own local optimums near it's global optimum. So, your method should quickly find a pretty good solution, while mine takes a bit longer (but trivially so) while finding a better solution. Exhaustively trying every combination of every joint is the only way to guarantee the global optimum for the best solution, but that really would be prohibitively slow. Another approach is to start with "polling", say virtually moving the first joint into each of 10 evenly spaced positions, then taking the best of those and subdividing the increments before and after by 1/10th the former size, and repeating until you get down to the minimum steps size. As before, repeat for each subsequent joint. This is similar to your approach but rather than subdividing by 2 we use a larger number. The larger the number the more calcs, which affects the trade-off between most optimal solution and quickest calcs.

Now if you want straight line movement of the tip from the start to end positions, that does rather complicate things. First, it may not always be possible, because some joint would need to go past it's limit to continue, so must now turn back all the way. When it is possible, I would expect you would need to find the target intermediate points, and repeat our method of finding the optimal joint positions for each. However, for each target point, rather than starting the calcs with each joint at 0, you would start the calcs with each joint where it was for the last target point, then go up or down by the minimal step size to optimize. As for dealing with hitting joint limits, if that happens, then we need to use a different solution for the starting position. For a screw-type joint, it may well rotate more than 360 degrees, say from 0 to 720. In that case, if our calcs show starting at 1 degree causes us to hit the limit at one of the intermediate points, we may want to start at 361 degrees, instead. StuRat (talk) 15:04, 22 July 2016 (UTC)[reply]