def start_sequence(psm, group):
# check whether or not the vessel is in the field of view
dv = get_vessel_location(15)
# if the vessel was not detected
if dv == False:
print "Vessel is outside the probe's FOV, should continue searching"
while dv != True:
# TODO: Proceed with the search algorithm that makes PSM scan the area
# should be checking for the vessel's presence in the background while searching is underway
dv = get_vessel_location(True)
print "Still searching..."
time.sleep(0.1)
# if the vessel is in the ultrasound's field of view, save the slice and save its deviation from the center
if dv != False:
print "Vessel is on the centerline, saving the slice"
save_slice()
# save the vessel's current deviation from the centerline
dev_Q = get_vessel_location()
# update pos_Q with the PSM's current position before sending it to exploration coords
pos_Q = mm.read_display_dvrk_pos(psm, verbose=False)
# ---------------
# TODO: This loop currently happens 10 times.
# It should be a while loop using a flag that indicates when it's time to stop
for iters in range(0, 10):
if iters == 0:
# if this is the first exploration journey,
# find the coordinates of the 4 exploration points
exp_coords = find_exploration_points(psm, group, increment=15)
else:
# now we have moved closer to the vessel, we only want to find 3 exploration
# coordinates, the fourth would bring us back to where we were before.
exp_coords = find_exploration_points(psm,
group,
increment=15,
exclude_position=pos_Q)
# update pos_Q with the PSM's current position before sending it to more exploration coords
pos_Q = mm.read_display_dvrk_pos(psm, verbose=False)
# save the vessel's current deviation from the centerline
dev_Q = get_vessel_location()
# visit the exploration coords and (hopefully) stop at one that gets the probe closer to the vessel
exp_result = explore(psm, exp_coords)
# if the exploration was successful
if exp_result:
print "Successful exploration, moved in the right direction"
else:
print "Unsuccessful exploration, none of the explored coords moved the probe in the right direction"
def find_exploration_points(psm, group, increment):
# find the arm's position in 3D
cur_XYZ = mm.read_display_dvrk_pos(psm, verbose=False)
# convert the increment given from mm to meters
inc_m = float(increment) / 1000
# +/- in Y
up_y = [cur_XYZ[0], cur_XYZ[1] + inc_m, cur_XYZ[2]]
down_y = [cur_XYZ[0], cur_XYZ[1] - inc_m, cur_XYZ[2]]
# -/+ in X
down_x = [cur_XYZ[0] - inc_m, cur_XYZ[1], cur_XYZ[2]]
up_x = [cur_XYZ[0] + inc_m, cur_XYZ[1], cur_XYZ[2]]
exploration_coords = [up_y, down_y, down_x, up_x]
return exploration_coords
def find_exploration_points(psm, group, increment, exclude_position=False):
# find the arm's position in 3D
cur_XYZ = mm.read_display_dvrk_pos(psm, verbose=False)
# convert the increment given from mm to meters
print "increment is", increment
inc_m = float(increment) / 1000
print "inc_m is", inc_m
# +/- delta increment in Y
up_y = [cur_XYZ[0], cur_XYZ[1] + inc_m, cur_XYZ[2]]
down_y = [cur_XYZ[0], cur_XYZ[1] - inc_m, cur_XYZ[2]]
# -/+ delta increment in X
down_x = [cur_XYZ[0] - inc_m, cur_XYZ[1], cur_XYZ[2]]
up_x = [cur_XYZ[0] + inc_m, cur_XYZ[1], cur_XYZ[2]]
if exclude_position == False:
exploration_coords = [up_y, down_y, down_x, up_x]
else:
# Of down_y, up_y, down_x and up_x, find the closest one to provided coordinate
exploration_coords = exclude_closest_coord(
target=exclude_position, options=[up_y, down_y, down_x, up_x])
return exploration_coords
sphere_pose.pose.position.x = x_in
sphere_pose.pose.position.y = y_in
sphere_pose.pose.position.z = z_in
scene.add_sphere(object_name, sphere_pose, radius=0.025)
if __name__ == '__main__':
# init and home
psm,group,scene = mm.init_and_home()
# allign both dvrk and MC
mm.update_mc_start_go(psm,group)
# read psm 3D coordinate
_ = mm.read_display_dvrk_pos(psm,verbose=True)
# add object(s)
raw_input("Adding the sphere...")
add_sphere(scene,frame_id="psm_tool_tip_link", object_name="sphere1")
# generate a trajectory which would normall hit the obstacle in the way
cur = mm.read_display_dvrk_pos(psm)
# set a goal position to move diagonally (10cm in negative Y and 10cm in negative X)
traj = mm.generate_traj(p=psm,goal_coords_list=[[cur[0]-0.1, cur[1]-0.1, cur[2]]],total_points=5000,group=group,fixed_orientation='vertical')
# start reading the goal+ actual positions in the background
logger = start_recording(psm)
# play the traj
print('Stopped recording, saved file.')
if plot:
print("Will plot the recording...")
import read_recording as rr
rr.main(recording_filename, plot_title="")
# =====================================================================
# =====================================================================
if __name__ == '__main__':
# - init and home
psm, group, _ = mm.init_and_home()
# - read psm 3D coordinate
xyz = mm.read_display_dvrk_pos(psm, verbose=False)
# - simulate OPSM pose, assume it is a few cm away and rotated 40 degrees
OPSM_sim_xy = [xyz[0] + 0.03, xyz[1] + 0.02]
OPSM_sim_rot = 40
# - provide that to bounding_rectangle and retrieve waypoints for a search path
_, path = br.main(rec_width_in=0.10,
rec_height_in=0.05,
gripper_location=OPSM_sim_xy,
gripper_rotation_degrees=OPSM_sim_rot,
gripper_displacement=0.3,
search_columns=search_cols,
height_padding=0.2,
plot=True)
# define the joint names
start_state.joint_state.name = ['psm_yaw_joint', 'psm_pitch_joint', 'psm_main_insertion_joint', 'psm_tool_roll_joint', 'psm_tool_pitch_joint', 'psm_tool_yaw_joint']
if __name__ == '__main__':
# initialise the required objects
psm,group = mm.init_dvrk_mc()
### move robot to home position
raw_input('Initialisation completed. Press ENTER to home the PSM')
psm.home()
time.sleep(1)
# read 3D pos from dvrk
raw_input('Press ENTER to read current position')
cp = mm.read_display_dvrk_pos(psm,verbose=True)
mm.read_display_dvrk_rot(psm,verbose=True)
mm.read_display_dvrk_angles(psm,verbose=True)
# Move the final 3 joints
raw_input('Press ENTER to move final 3 joins')
# move all joints - relative, incremental in joint space
psm.dmove_joint(np.array([0.0, 0.0, 0.0, -0.05, 0.4, 0.25]), interpolate = True)
# read 3D pos from dvrk
raw_input('Press ENTER to read current position')
mm.read_display_dvrk_pos(psm,verbose=True)
mm.read_display_dvrk_rot(psm,verbose=True)
mm.read_display_dvrk_angles(psm,verbose=True)
def save_slice():
psm_position = mm.read_display_dvrk_pos(psm, verbose=False)
# TODO: Save the slice in the directory shared with the 3D reconstruction block.
# Set the filename to be the read position of the PSM
print "Ultrasound slice saved, arm is at", psm_position
