Method of calculating points and normal lines of contact and apparatus therefor

A method of determining a contact point and a normal vector at the contact point between a tool or part attached at an end of a multi-jointed robot arm and a workpiece or object fixed on a table of a robot machine, the robot being capable of detecting and recording three-dimensional position, orientation, velocity, and angular velocity of said tool or part, the normal vector being normal to a surface of said tool or part at said contact point, the method comprising the steps of:

• storing data related to geometrical configuration of said tool or part in a computer memory;
• recording an opposing force applied to said tool or part, said opposing force being detected by a force detector on said robot arm;
• inputting into the computer memory information related to three-dimensional positions and angular velocities of said tool or part and of joints on said robot arm, and information related to said recorded opposing force applied to said tool or part;
• calculating a force application vector based on said opposing force;
• calculating an intersection point of said force application vector with said tool or part surface from said geometrical configuration of said tool or part;
• selecting multiple points on the surface of said tool or part in an area around said intersection point, calculating a normal vector at each of said multiple points normal to the surface of said tool or part, calculating a translation velocity vector for each of said multiple points from said angular velocities of said joints, and calculating a dot product of said normal vector and said translation velocity vector for each of said multiple points;
• calculating for each of said multiple points a boundary distance, said boundary distance being a product of said normal vector and a distance from each of said multiple points to a point representing a boundary area;
• calculating a distance of said force application vector, said distance of said force application vector being the distance between each of said multiple points and said intersection point;
• calculating a determination function for each of said multiple points from said dot products, said boundary distance, and said distance of said force application vector;
• calculating said contact point among said multiple points and determining the normal line at said point of contact between said tool or part and said workpiece, said contact point being calculated by choosing a point where said determination function and the corresponding normal vector at said contact point normal to said tool or part surface are minimized;
• and
• outputting contact point and normal line information to a controller controlling the movement of said robot arm, such that said robot arm can position said tool or part.