How to define a Reference Frame with 6 points using Python?

Hello fellow robotic enthusiasts! :globe_with_meridians::robot:

Let’s dive into the intriguing world of defining reference frames in robotics. There are multiple methods out there, but today, I want to shed some light on the 3-2-1 method, a personal favorite of mine.

The 3-2-1 Method:

In this method, we use a combination of points to establish a reference frame:

  • 3 points define the top plane.
  • 2 points contribute to an adjacent plane.
  • And finally, 1 point locks in the last plane.

It’s like connecting the dots in space! :milky_way: This method offers a structured and intuitive way to set up a reference frame, crucial for accurate robotic movements.
Capture d’écran 2024-01-05 142427

In that scenario, here’s a code example that will output a reference frame in multiple formats.

# Here's where you apply the TCP you will use during probing
is_successful = robot.set_tcp_offset(toolingOnly_offset)
print("TCP for Calibration: ",is_successful)

### Input the 6 probed points here ###
pointList = np.array([
    [255.63,-601.24,-49.20,-100.99,87.19,-170.23],# A
    [-147.10,-746.40,-43.68,89.46,60.19,-0.67],# A
    [-204.32,-523.36,-50.08,-92.06,-53.36,179.83],# A
    [350.15,-491.66,-72.55,-92.81,-0.40,-89.82],# B
    [352.25,-697.07,-64.41,99.11,1.33,-89.47],# B
    [196.74,-378.35,-61.60,178.99,-2.10,179.75] # C

### Reference Frame Calibration Section ###

def getReferenceFrame(points):
    # plan A
    pt1 = points[0,:3]
    pt2 = points[1,:3]
    pt3 = points[2,:3]
    v21 = pt2-pt1
    v23 = pt2-pt3
    NormalA =np.cross(v21,v23)
    NormalA = NormalA / np.linalg.norm(NormalA)
    Da =,pt1)
    print(f"plan A equation: {NormalA[0]:.2f}x + {NormalA[1]:.2f}y + {NormalA[2]:.2f}z = {Da}")
    # plan B
    pt4 = points[3,:3]
    pt5 = points[4,:3]
    ptb = pt4 + NormalA
    v54 = pt5-pt4
    v5b = pt5-ptb
    NormalB =np.cross(v54,v5b)
    NormalB = NormalB / np.linalg.norm(NormalB)
    Db =,pt4)
    print(f"plan B equation: {NormalB[0]:.2f}x + {NormalB[1]:.2f}y + {NormalB[2]:.2f}z = {Db}")

    #plan C
    pt6 = points[5,:3]
    ptc1 = pt6 + NormalA
    ptc2 = pt6 + NormalB
    v6c1 = pt6 - ptc1
    v6c2 = pt6 - ptc2
    NormalC =np.cross(v6c1,v6c2)
    NormalC = NormalC / np.linalg.norm(NormalC)
    Dc =,pt6)
    print(f"plan C equation: {NormalC[0]:.2f}x + {NormalC[1]:.2f}y + {NormalC[2]:.2f}z = {Dc}")

    # find interesction of the 3 planes  (solve Ax = b)
    A = np.array([NormalA, NormalB, NormalC])
    b = np.array([Da, Db, Dc])

    # Solve the system of linear equations
        origin = np.linalg.solve(A, b)
    except np.linalg.LinAlgError:
        # The planes are parallel or coincident, no unique solution
        return None

    # Z axis, along NormalA, check if pointing up or down with pt4
    def adjustOrientation(Normal,pt):
        dir = pt - origin
        upOrDown =,Normal)
        if upOrDown < 0:
            return -Normal
            return Normal
    zAxis = -adjustOrientation(NormalA,pt6)  # z axis is inverted by convention
    xAxis = adjustOrientation(NormalB,pt6)
    yAxis = adjustOrientation(NormalC,pt1)
    print("normalA (Z)",NormalA,zAxis) 
    print("normalB (X)",NormalB,xAxis)
    print("normalC (Y)",NormalC,yAxis)

    rot = np.zeros( (3,3))
    rot[:,0] = xAxis
    rot[:,1] = yAxis
    rot[:,2] = zAxis
    r = scipyR.from_matrix(rot)
    print("Rotation Matrix:",rot)
    eulr = r.as_euler('xyz', degrees=True)
    return [origin[0],origin[1],origin[2],eulr[0],eulr[1],eulr[2]]

result = getReferenceFrame(pointList)

Why I Like It:

I find the 3-2-1 method provides a nice balance between simplicity and accuracy. It’s especially handy when working on tasks where a well-defined reference frame is essential for success.

Your Thoughts?

Have you explored different methods for defining reference frames? What’s your go-to approach? Share your experiences or any tips you might have! Let’s exchange ideas and knowledge. :thinking::speech_balloon: