Move TCP to Midpoint Between Two Contact Points

I’m using a series of Direction / Until Contact nodes to find edges so I can more easily set waypoints.

I need to be able to move to a position that is centered between two points that I found using Direction / Until Contact.

So basically I want to:

Direction X+
  Until Contact
    Store Position1
Direction X-
  Until Contact
    Store Position2
Position3 = Position2 - Position1
Move Position3

How would I store Position1 and Position2 so I can derive Position3?

I’m going to use CASE selections for the user to pick which axis he wants to use (X, Y or Z) so once I get one axis figured out I will need to be able to replicate it in the other 2 axis.

UR10e running in Polyscope 5.10.xx



def midpoint(first, second):
  mid = get_actual_tcp_pose()
  mid[0] = (first[0] + second[0]) / 2
  mid[1] = (first[1] + second[1]) / 2
  mid[2] = (first[2] + second[2]) / 2
  return mid

Add a script file into your before start sequence and type in/paste that into it. Then use an Assignment node to store the points after contact, and use another Assignment node to call the script and pass in the 2 stored points. You can use this newly assigned Midpoint just like any other waypoint, and just use a Variable waypoint and choose that one.


I got a little ahead of myself actually and didn’t read your post all the way through. This will find the midpoint between any 2 poses. If you want to do only one axis at a time or something, feel free to modify the script.


Awesome, thanks AGAIN, Eric.

Finishing up runs on both robots so will by checking this out today.


Added to one robot so far, works perfect. :+1:

1 Like

Hey @eric.feldmann,

Does this work on the CB series or is the ‘Until contact’ node only an e series feature.



I don’t know anything about the CB series unfortunately. We only have e-Series here. This can get the midpoint between any 2 points, it doesn’t have to be via Tool Contacts, if that makes a difference to you.

That said, here’s the script that gets generated by the e-series. Not sure if you have the ability to write your own script in CB or not:

def calculate_point_to_move_towards(feature, direction, position_distance):
    local posDir=[direction[0], direction[1], direction[2]]
    if (norm(posDir) < 1e-6):
      return get_target_waypoint()
    local direction_vector_normalized=normalize(posDir)
    local displacement_pose=p[direction_vector_normalized[0] * position_distance,direction_vector_normalized[1] * position_distance,direction_vector_normalized[2] * position_distance,0,0,0]
    local wanted_displacement_in_base_frame=pose_sub(pose_trans(feature, displacement_pose), feature)
    return pose_add(get_target_waypoint(), wanted_displacement_in_base_frame)
  while (True):
    $ 1 "Robot Program"
    $ 2 "MoveL"
    $ 3 "Direction: Base X+"
    global move_thread_flag_3=0
    thread move_thread_3():
      move_thread_flag_3 = 1
      local towardsPos=calculate_point_to_move_towards(p[0.0,0.0,0.0,0.0,0.0,0.0], [1.0,0.0,0.0], 1000.0)
      movel(towardsPos, a=1.2, v=0.050799972568014815)
      move_thread_flag_3 = 2
    move_thread_flag_3 = 0
    move_thread_han_3 = run move_thread_3()
    while (True):
      local targetTcpDirection=get_target_tcp_speed()
      local stepsToRetract=tool_contact(direction=targetTcpDirection)
      if (stepsToRetract > 0):
        kill move_thread_han_3
        local backTrackMovement=get_actual_joint_positions_history(stepsToRetract)
        local contactPose=get_forward_kin(backTrackMovement)
        local posDir=[targetTcpDirection[0],targetTcpDirection[1],targetTcpDirection[2]]
        local retractTo=contactPose
        if (norm(posDir) > 1e-6):
          local normalizedPosDir=normalize(posDir)
          local additionalRetraction=p[normalizedPosDir[0] * 0.0, normalizedPosDir[1] * 0.0, normalizedPosDir[2] * 0.0, 0, 0, 0]
          retractTo = pose_sub(contactPose, additionalRetraction)
        movel(retractTo, a=3.0, v=0.1)
        $ 4 "Until (tool_contact_detection)"

No worries thanks Eric.

I might play around with it in the URSim when i need it and see what i come up with.