Position Differences

[SkyeFire]

Must be serendipity -- this new board gets created this week, and today I suddenly have an unanticipated need for something related.

I'm running a KUKA KRC2 that is using the RSI force-torque system to "feel" its way into normality with a contact surface.  That is, the tip of the end effector comes into contact with a surface under some small pressure, and a multi-direction strain gauge that's part of the end effector mounting detects torques generated by the tip not being perfectly flat against the work surface.  The RSI drives the robot to rotate around the TCP until (ideally) all the torques are 0.

Thing is, due to the nature of Euler angles, I can't tell how much the robot is rotating.  My ABC display shows the the angle changing from 14,89,14 to something like 178,88,178, even though I can *see* that the rotation is only 2-3degs.  I figure there must be a fairly straightforward way of taking two different positions and calculating what their orientation differences are per axis, probably by reversing the the way that the Geometric Operator takes a position and rotates it by a desired amount.

(I'm going to have to go back to school and re-take Matrix Algebra, I can *see* it coming.  I wonder if anyone offers a good online course...?)

[asimo]

Hi SkyeFire,

You can find the angles between axes by using the 'magical mystery matrix' that I've given here:

http://www.robot-forum.com/robotforum/robot_geometry_linear_algebra_forward_and_inverse_kinematics/calculating_position-t1246.0.html

By using that matrix, put a zero for the translations and your A,B,C angles that you have. You will have to use it twice. Once for the first position and a second time for your second position. From the result you have from both matrices, the first column represent the X unit vector, the second column the Y unit vector and the third the Z unit vector. You now have vector matrices for each axis by removing the last line with the zero value. So by using the 2 vectors for each axis, you can find the angle using this formula:

Code:

arccos(v1 . v2)

The 'dot' in between v1 and v2 means you have to use the "dot product" for matrices which gives the projection of one vector into another.

I hope this helps.