If the End Point is on the target, then you don't want the structure to move any more, but if the End Point is very far from the target, then you want the structure to move there quickly. Again, I decided that the torque should also be proportional to the magnitude of the force vector. One thing the previous two paragraphs do not tell you is which direction the joint needs to move. So the joint should move in a positive direction of the force points along r , and in a negative direction of the force points in the opposite direction to r . Sensitivity Where SinVect and CosVect return the sin and cos of the angle between two vectors respectively, Mag returns the length of a vector, and sign returns the sign of a number.
Sensitivity is some small scaler constant.
So then you move onto the next joint up the structure. Re-calculate the vectors a , b and f , and calculate the torque for joint 2. Calculate the torque for all the joints in the whole structure, then add the torque to each joint to it's angle. The End Point of the structure should now be closer to the target. Repeat the process continuously, and it should do it's best to keep touching the target.