# Finding correct RPY for approaching a known point on a plane

I have a robot with 6 DOF that I want to use to grab a hanging sheet at a specific point near its edge. The grip needs to be perpendicular to the edge and co-planar with the point of course.

I have the inverse kinematics and I have the XYZ-coordinate that I want to grab, however, I'm struggling a bit with finding the correct RPY needed to approach the point correctly. I have looked at the post How to calculate roll, pitch and yaw from XYZ coordinates of 3 planar points? on Mathwork Matlab Answers, which helped me find the normal to the plane, but there's something I'm not doing or understanding correctly in regards to the calculation of the RPY.

Here's a sketch of the problem:

P2 is the point I want to grab and the arrow illustrates the angle of approach I want to achieve. I know the angle (theta), but none of my robot's axes align with this axis of rotation so I can't simply add this.

Here:

I tried to follow the instructions in the MathWorks post above, ending up with a sketch of the sheet's coordinate frame looking like this:

(Z points into the plane).

Next I calculate the alpha, beta and gamma values like suggested in the post. But what do I do with these?

If I plug the results into my robot the TCP's angle is not even coplanar. I know I haven't taken the theta angle from the sketch into account yet, but I don't want to consider that until I see the TCP is coplanar with the sheet.

Can anyone give me a clue on what I'm doing wrong or if I'm missing some steps? Maybe there's even a better way of achieving what I want than what I tried here? And let me know if I need to clarify anything - this is my first post here, so please be gentle.

Here's the code I have written to do the calculations. p1t, p2t and p3t are the three homogeneous coordinates P1, P2, and P3 respectively in the robot's frame (what I call the global frame):

// Transform coordinates from cam to robot
Eigen::Vector4f p1t = H * p1;
Eigen::Vector4f p2t = H * p2;
Eigen::Vector4f p3t = H * p3;

Eigen::Vector3f p1t3D, p2t3D, p3t3D;
p1t3D << p1t(0), p1t(1), p1t(2);
p2t3D << p2t(0), p2t(1), p2t(2);
p3t3D << p3t(0), p3t(1), p3t(2);

Eigen::Vector3f v1, v2;
v1 = p2t3D - p1t3D;
v2 = p3t3D - p1t3D;

Eigen::Vector3f xUnitVec = ((p1t3D + p2t3D)/2) - p3t3D;
xUnitVec.normalize();

Eigen::Vector3f zUnitVec = v1.cross(v2);
zUnitVec.normalize();

Eigen::Vector3f yUnitVec = zUnitVec.cross(xUnitVec);

std::cout << "p2t: \n" << p2t << std::endl;

float x1 = xUnitVec(0);
float y1 = yUnitVec(0);
float z1 = zUnitVec(0);
float z2 = zUnitVec(1);
float z3 = zUnitVec(2);

float alpha = atan2(-z2, z3);
float beta = asin(z1);
float gamma = atan2(-y1, x1);

std::cout << "\nRotations: " << std::endl;
std::cout << "Alpha: " << alpha;
std::cout << ", Beta: " << beta;
std::cout << ", Gamma: " << gamma;

pickPoint.clear();
pickPoint = {p2t(0), p2t(1), p2t(2), alpha, beta, gamma};

• What type of robot are you using? it is important the match the RPY definition of your robot, since some robtos interpret the angles slightly differentlty. – 50k4 Mar 26 '19 at 10:11
• It is a Universal Robots UR5. Now that you say it, I may have been mistaken about needing RPY. The manual mentions rotation vectors Rx, Ry, Rz (I assume that's the column vectors of a rotation matrix?). Explanation from manual I'm still not sure how to approach this, however. Am I correct in saying I need a transformation from the plane's frame to the global frame? How would I do that? Thank you for your comment! – asdfghjkl Mar 26 '19 at 11:01
• A lenghty explanation of the angles can be found here. If I have time to identify the correct rotation sequence I will turn to comments to an answer. It is not clear to me if these are intrinsic or extrinsic rotations. zacobria.com/universal-robots-zacobria-forum-hints-tips-how-to/… – 50k4 Mar 26 '19 at 12:33
• The base frame is used as a reference frame for the 3 angles. you can use matlabs dcm2angle with the required rotation axes (probably ZYX) and it will convert your rotation matrix to a set of angles and this explaines how to build your DCM matrix from a coordinate system definition starlino.com/wp-content/uploads/data/dcm_tutorial/… – 50k4 Mar 26 '19 at 12:35
• Perhaps this could be helpful: Screenshot from UR script manual. The rotation vector refers to the axis angle vector it seems. And the RPY rotation is extrinsic in XYZ order. – asdfghjkl Mar 26 '19 at 14:18

It seems that you have successfully identified the RPY angles. In the comments it was made clear that the UR5 robot you are using expects the orientation as an XYZ extrinsic rotation (which corresponds to an ZYX intrinsic rotation).

As per the discussion in the comments the UR5 controller offers a function to convert RPY angles to the requires extrinsic XYZ representation called rpy2vecrot(). This function should be uest to convert the orientation angles calculated by you to the required format.

• Thanks for your answer, however the conversion between RPY and rotation vector is not my problem - I knew about the internal conversion functions before posting here. The problem is finding the rotation of the plane w.r.t. the global frame. When I give the robot the results of my calculations detailed in the original post, it is not behaving as expected. I would like the TCP to be co-planar with the sheet, however it is not. What I would like to know is if the process I'm applying is correct for my problem. – asdfghjkl Mar 27 '19 at 21:21
• I added the code I used to make these specific calculations to my OP. – asdfghjkl Mar 27 '19 at 21:31

Using Euclidian Angles normally can be confusing because the order of rotation interfers directly on the final rotation. As said above, Universal Robot series uses a specific order of rotation that you have to match.

Besides, if you're using movel ou movej with the UR5, you have to know that the Rx, Ry and Rz represents an Axis-Angle representation. In modern robots, axis angle or quaternion is normally used instead of Euclidian because of Gimbal lock. I believe you can find the specific code to transform from one to another in the Universal Robot forums (Also in early versions the Axis-Angle you see in the screen is not the same you receive via Ethernet, although they represent the same rotation).

Keep in mind that those positions and angles need to be represented in relation to the UR5 frame.

If you have the angle of the plane and a gripper to get the sheet, you can have the normal axis of the gripper match the normal axis of the sheet, then it becomes a grasping problem.

• Yes, there is indeed a function implemented to convert between RPY and axis angle in the UR software. However, I have tried entering the coordinates I calculated manually into the UR pendant, where I can select the format to be either RPY or Rotation vector (aka. axis-angle rotation). Neither of these produce the correct result unfortunately. I'm worried that the calculation of the vectors might be wrong. If you look at the pictures in my original post, notice how I'm shifting the X-axis to originate from P1 instead of P3. Is that a problem? Is there a better way to solve this problem? – asdfghjkl Apr 4 '19 at 22:56
• In this case, it's probably a transformation. Have you tried calculating the rotation before transforming? You'll find the rotation in relation to the camera frame and after that you transform from the camera to the robot. Another option is to transform from matrix directly into rv to avoid representation confusion in the order of the RPY. – Felipe Henrique Apr 5 '19 at 12:30
• Can you go into more detail (or do you have some resources on the subject) on finding the rotation between cam and robot? They only share one parallel axis between the two. I'm not sure why I need that rotation, when I can have XYZ robot coordinates. That's why I thought it would be best to simply find the translation between the two, which is easily done by applying SVD to ~20 measured 3D points. Then my plan was to apply the "technique" in the Matlab post with the transformed points (P1, P2, P3 from the sketches) in robot coordinates, and find the plane's rotation w.r.t. the robot frame. – asdfghjkl Apr 6 '19 at 20:21
• That's a topic on it's on. It's normally called Hand-To-Eye Calibration, and it's used to ensure that you can transform what you see from the camera to the robot frame, having the transformation from robot to camera. I answered something similar here, if you want to take a look. – Felipe Henrique Apr 7 '19 at 16:20
• There aren't many ways to do it automatically, and as far as I know, for experimental setups, it's done by hand, normally using a common point accessed from robot and camera. – Felipe Henrique Apr 7 '19 at 16:22