Single Linear Axis Calibration

This script can be used to calibrate a linear axis (as an example). It follows a specific method of sampling points along the linear axis. It can be run as robot “mounted” on the saddle or chassis of the axis (cobot on rail) or with the robot off the axis. Set the value for MOUNTED accordingly in the script at the end.

Mounted

When the robot is mounted to the axis, we sample the origin, then the point marked X. For the next two points, we move the robot (on the axis) to different positions down the axis. See figure 1 below, where the blue block represents the 3 positions for the saddle to which the robot is mounted. The origin is sampled first, then the point X to indicate x-axis direction, then the origin twice more with the robot at position 2 and position 3.This results in poses {origin, px1, pz2, pz3}

Not Mounted

If the robot is not mounted to the linear axis, we still need to know the position of the axis relative to the robot base and the world coordinate system. In this case, we move the saddle (chassis) and sample the points on the chassis itself, as in figure 2 below:

This script will step the user through calibration on the Teach Pendant by sampling multiple points along the axis, known as origin, px1, pz2 and pz3. For a linear external axis, the direction of motion is along the Z axis.

# EtherCAT Single Linear Axis Config Script

# NOTE: You must first create the following global variable 
# in the installation tab under "General"
#       CAL_AX_LINEAR  p[0,0,0,0,0,0]
# This will be used to store the calibrated pose calculated here

PI = acos(-1)

AXIS_TYPE = 1                # 0:rotary, 1:linear

# These values should be according to your linear axis setup, servo drive and motor
VELOCITY_LIMIT = 0.500       # m/s
ACCELERATION_LIMIT = 1.0     # m/s^2
ENCODER_RESOLUTION = 1000000
FEED_CONSTANT = -1           # Pulley diameter * PI
GEAR_RATIO =  1              # Output gear
ZERO_OFFSET = 0              # Offset from homed zero position

RAIL_MIN = 0.0
RAIL_MAX = 1.0

def my_axis_ecat_config():
    # stop EtherCAT master first, otherwise, reset_world_model may throw an error
    ethercat_clear_error()
    ethercat_stop(True)
    reset_world_model()

    axis_group_add("rail", p[0,0,0,0,0,0], "world")
    axis_group_add_axis("rail", "axis1", "", p[0,0,0,0,0,0], AXIS_TYPE, VELOCITY_LIMIT, ACCELERATION_LIMIT, [RAIL_MIN, RAIL_MAX])

    ethercat_config_axis("axis1", 1, ENCODER_RESOLUTION, GEAR_RATIO, FEED_CONSTANT, ZERO_OFFSET)

    ethercat_start(10)
    sleep(1) # wait for the ethercat to start

    if (ethercat_is_axis_in_fault_state("axis1")):
        textmsg("FAULT in: ", "axis1")
        if(ethercat_reset_axis_fault("axis1")):
            textmsg("Axis 1 FAULT cleared")
        else:
            return False
        end
    end
    ethercat_enable_axis("axis1")
end

def my_update_axis(is_mounted=False):
    # If the robot is mounted to the linear axis, update the axis position,
    # then move the robot base to the axis and rotate it.
    ethercat_disable_axis("axis1")
    if (is_mounted):
        # PI flips the Z axis direction by rotating around X 180 degrees
        rotated_pose = pose_trans(p[0,0,0,PI,0,0], CAL_AX_LINEAR)
        
        tx = rotated_pose[0]
        ty = rotated_pose[1]
        tz = rotated_pose[2]

        rx = rotated_pose[3]
        ry = rotated_pose[4]
        rz = rotated_pose[5]

        axis_group_update_axis(name="axis1", pose=p[tx,ty,tz,0,0,0])
        move_frame("base",p[0,0,0,rx,ry,rz], "axis1")
        attach_frame("base", "axis1")
    else: #not mounted
        axis_group_update_axis(name="axis1", pose=CAL_AX_LINEAR)
    end
    ethercat_enable_axis("axis1")
end

# Single Rotary Axis Calibration
def move_axis(group_name, axis1_position):
    msg1 = group_name
    msg1 = str_cat(msg1, " will now move to [")
    msg1 = str_cat(msg1, to_str(axis1_position))
    msg1 = str_cat(msg1, "] meters")

    popup(msg1, "Axis is about to move!", warning=True, error=False, blocking=True)

    axis_group_movej(group_name, [axis1_position], 1, 1)
end

popup_title = "Linear Axis Calibration"

def teachCalPointAxis(position1, ref_frame, sample_point_name):
    if (sample_point_name != "X direction"):
      move_axis("rail", position1)
    end
    sleep(0.5)
    freedrive_mode()
    msg = str_cat("Freedrive to the ", sample_point_name)
    msg = str_cat(msg, " point, then press Continue")
    popup(msg, title=popup_title, blocking=True)
    p = get_pose("tcp", ref_frame)
    msg = str_cat("Freedrive up and away from the ", sample_point_name)
    msg = str_cat(msg, " point, then press Continue")
    popup(msg, "Axis Calibration", blocking=True)
    end_freedrive_mode()
    return p
end

#-----------------------------------------------------------
# Provide a function to configure your base ethercat setup
#-----------------------------------------------------------

def my_calibrate_linear_axis(mounted=False):

    # Teach the pose of axis1 relative to the robot base.
    # The robot base should not move
    popup("Ready to Calibrate Axis1?", title=popup_title, warning=False, error=False, blocking=True)

    popup("Step 1: Home the Linear axis. Ready?", title=popup_title, warning=False, error=False, blocking=True)

    # Homing - Choose an appropriate homing method and velocities for your system
    # NOTE: Ensure that the positive direction for homing matches the positive movement direction of the axis. 
    homing_method = -2 # home to block
    homing_offset  = 0 # [encoder counts]
    homing_velocities = [100, 100] # [encoder counts / sec]
    homing_acceleration = [50, 50] # [encoder counts / sec^2]

    ethercat_home_axis("axis1", homing_method, homing_offset, homing_velocities, homing_acceleration)

    AXIS_LENGTH = 1  # length in meters

    if (mounted == True):
        # Applying an origin offset helps move the base away from the origin so
        # that the TCP can reach the origin
        ORIGIN_OFFSET = 0.2*AXIS_LENGTH
    else:
        # Do not apply an offset if the robot is not mounted
        ORIGIN_OFFSET = 0
    end 

    # Teach the origin point
    pz1 = teachCalPointAxis(ORIGIN_OFFSET, "base", "origin")
    origin_offset = get_pose("axis1", "world")[2]

    # Teach the X axis direction (single point)
    popup("Teach X direction point", title=popup_title, warning=False, error=False, blocking=True)
    px1  = teachCalPointAxis(ORIGIN_OFFSET, "base", "X direction")

    # Teach two more points along the linear axis
    pz2 = teachCalPointAxis( ORIGIN_OFFSET + AXIS_LENGTH*0.2, "base", "origin")
    pz3 = teachCalPointAxis( ORIGIN_OFFSET + AXIS_LENGTH*0.4, "base", "origin")

    # [0] index is the pose within the struct returned by calibrate_linear_axis
    CAL_AX_LINEAR = calibrate_linear_axis(pz1, [pz2, pz3], px1, mounted)[0]

    # Subtract any origin offset along the Z direction (the axis length)
    z_adjustment = CAL_AX_LINEAR[2] + origin_offset
    CAL_AX_LINEAR[2] = z_adjustment

    popup("Calibration has been saved.", title=popup_title, warning=False, error=False, blocking=True)
end

mounted = True
#for calibration, run this:
my_axis_ecat_config()
my_calibrate_linear_axis(mounted)
my_update_axis(mounted)

# after calibration, this is all you need
#my_axis_ecat_config()
#my_update_axis(mounted)