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
def midpoint(first, second): mid = get_actual_tcp_pose() mid = (first + second) / 2 mid = (first + second) / 2 mid = (first + second) / 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.
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.
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, direction, direction] if (norm(posDir) < 1e-6): return get_target_waypoint() end local direction_vector_normalized=normalize(posDir) local displacement_pose=p[direction_vector_normalized * position_distance,direction_vector_normalized * position_distance,direction_vector_normalized * 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,targetTcpDirection,targetTcpDirection] local retractTo=contactPose if (norm(posDir) > 1e-6): local normalizedPosDir=normalize(posDir) local additionalRetraction=p[normalizedPosDir * 0.0, normalizedPosDir * 0.0, normalizedPosDir * 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
No worries thanks Eric.
I might play around with it in the URSim when i need it and see what i come up with.