Move TCP to Midpoint Between Two Contact Points

General Application Help: How do I?
1

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
Halt

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

Thanks!

2

Script:

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
end

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.

image

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.

2 Likes
3

Awesome, thanks AGAIN, Eric.

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

:edit:

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

1 Like
4

Hey @eric.feldmann,

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

Cheers,

Jarrah

5

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()
    end
    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)
  end
  while (True):
    $ 1 "Robot Program"
    $ 2 "MoveL"
    $ 3 "Direction: Base X+"
    global move_thread_flag_3=0
    thread move_thread_3():
      enter_critical
      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
      exit_critical
    end
    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
        stopl(3.0)
        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)
        end
        movel(retractTo, a=3.0, v=0.1)
        $ 4 "Until (tool_contact_detection)"
        break
      end
      sync()
    end
  end
6

No worries thanks Eric.

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

Cheers,

Jarrah