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