Finding correct RPY for approaching a known point on a plane

Question

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};

Answer 1

A plane is defined through 3 points that pass through it or by a single point and a perpendicular vector. There are formulas that describe this behavior. Then, that theta you drew in your first diagram looks to me like the angle-axis representation of a frame transform. You could then use Rodriguez formula to create a 3x3 transformation matrix of the rotation matrix that defines the approach angle. I suggest adding a 90 degree rotation about the perpendicular vector (whose XYZ location is at P2) to the theta angle to get the approach angle of your gripper. Does this make sense? I hope this helps.

Answer 2

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.

Answer 3

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.