Local posreg does not recognize the global default pose config

[Hamdan Al-Musaibeli]

The local register seems to use a different group configuration instead of the default. Here is an example.

import Tasks

#constents
GRIPPER_DEPTH := 75
PPC := 3   # points per circle


#variables
    part_rad        := LR[]
    farCir_pos      := LR[]
    closeCir_pos    := LR[]
    part2tof_dist   := LR[]
    centers         := LR[]

#end of variables


#user input:

    # workpiece information
    PART_DIA := 153
    PART_LEN := 576
    part_rad = PART_DIA/2

    # far and close circles positions
    FARCIR      := 10   #mm wrt part edge
    CLOSECIR    := 10   #mm wrt gripper edge

    #for approach target calculation
    farCir_pos = PART_LEN - (GRIPPER_DEPTH + FARCIR)
    closeCir_pos = farCir_pos - CLOSECIR

    # robot pose approach
    CLEARANCE := 0.03             # initial clearance between part and tof with respect to measure frame
    TOFAPPROACH_ANGLE := 0        # with respect to tof measure frame
    part2tof_dist = PART_DIA/2 + CLEARANCE

#end of user input


#preprocessing section

    # getting robot ready
    TP_GROUPMASK = "1,*,*,*,*"
    TP_COMMENT = "3D tcp correction"
    
    zero_pos := P[1]

    default.group(1).pose -> [0, 0, 0, 0, 0 ,0]
    default.group(1).config -> ['N', 'U', 'T', 0, 0, 0]

    #safe_pos := PR[12]

    #move to safe pose
    #joint_move.to(safe_pos).at(30, '%').term(FINE)


    # clear tool offset
    Pos::clrpr(&tool_ofset)

    #check safe pos
    #G0_SAFE_POS()

    #rotate J1 to 95 deg
    #G1_ROTT_J1(95)

    #G0_DOOR_OPEN()

    dummypr1 = base_gripper
    part_frame = dummypr1

    # move into approach
    use_uframe tcp_calib_frame
    use_utool base_gripper

#end of perprocessing section


#approach tof
Tasks::appro_tof(TOFAPPROACH_ANGLE, part2tof_dist, farCir_pos)

For the tasks module, it contains a local posreg which should have (NUT) config but instead uses (NBU) config.

    def appro_tof(tofApproach_angle, part2tof_dist, farCir)
        # perform proper robot motion to approach the tof sensor, until surface is at zero with respect to
        # the tof measure frame. This motion is only on the z-axis.

        TP_GROUPMASK = "1,*,*,*,*"

        #vars
        pr1 := LPR[]

        # building target approach pose for the first time only
        # when correcting for the second time we use initial pose
        #if not len(self.initCloseTrgt):
        pr1 = Pos::setxyz(-1*part2tof_dist, 0, -1*farCir, tofApproach_angle, 0, 0)
        
        #move to the target
        joint_move.to(pr1).at(20, '%').term(FINE)
    end

But it will work if we use and initialize global posreg as in this code:

    def appro_tof(tofApproach_angle, part2tof_dist, farCir)
        # perform proper robot motion to approach the tof sensor, until surface is at zero with respect to
        # the tof measure frame. This motion is only on the z-axis.

        TP_GROUPMASK = "1,*,*,*,*"

        #vars
        pr1 := P[60]
        pr1.group(1).pose -> [0, 0, 0, 0, 0 ,0]
        pr1.group(1).config -> ['N', 'U', 'T', 0, 0, 0]

        # building target approach pose for the first time only
        # when correcting for the second time we use initial pose
        #if not len(self.initCloseTrgt):
        pr1 = Pos::setxyz(-1*part2tof_dist, 0, -1*farCir, tofApproach_angle, 0, 0)
        
        #move to the target
        joint_move.to(pr1).at(20, '%').term(FINE)
    end

[Matt Dewar]

Hey @halmusaibeli, You are confusing positions with position registers in this case. The default.group(1).pose, and default.group(1).config in this case only pertain to positions, and not position registers. In def appro_tof you need to set the position register config like this:

pr1 := LPR[]
Pos::clrpr(&pr1)
pr1 = Pos::setxyz(-1*part2tof_dist, 0, -1*farCir, tofApproach_angle, 0, 0)
pr1 = Pos::setcfg('N U T, 0, 0, 0')

I've added default arguments for the pose TP interfaces. So there isn't a separate function for setxyz_grp anymore. You will have to update ka-boost to the latest commit (8a94c707dc565d466e9e6106b2ab53260c358a7c), and reexport the TP interfaces to the controller. This way you dont have to specify the group for Pos::setcfg (i.e. pr1.group(1) = Pos::setcfg('N U T, 0, 0, 0')).

rossum .. -w -o -i -b
ninja
kpush

[Hamdan Al-Musaibeli]

Thank you so much @kobbled.
It worked like a charm.