def left2right_motion(robot, left_point, center, right_point, multiplier):
# Move left and then move back with multiplier 2
robot.add_goal(cb2_robot.Goal(left_point, True, 'linear', radius=0.0))
while not robot.goals.empty():
robot.move_on_stop()
print("moved left!")
robot.add_goal(cb2_robot.Goal(center, True, 'linear', radius=0.0))
spliner = Spline(order=2)
path = spliner.get_path(left_point[:3],
center[:3],
right_point[:3],
n=100,
bezier=False)
#print(path)
new = center
new[2] = new[2] + 0.1
robot.add_goal(cb2_robot.Goal(new, True, 'linear', radius=0.0))
while not robot.goals.empty():
robot.move_on_error(multiplier=multiplier)
print("moved to the right with spline")
def center_robot(robot, pose):
# angle_start = [90, -95, 90, 0, 90, 90]
# angle_start = map(cb2_robot.cb2_send.deg_2_rad, angle_start)
angle_start = [
-01.5503467, -02.0665188, -01.3330899, +00.1346348, +01.6603317,
-01.5165228
]
robot.add_goal(cb2_robot.Goal(angle_start, False, 'joint'))
robot.add_goal(cb2_robot.Goal(pose, cartesian=True, move_type='linear'))
while not robot.goals.empty():
robot.move_on_stop()
def curve_motion(robot, current, goal, new):
spliner = Spline(order=2)
path = spliner.get_path(current[:3], goal[:3], new[:3], n=30, bezier=True)
print(path)
for p in path:
robot.add_goal(
cb2_robot.Goal([p[0], p[1], p[2], 1.2, -1.2, 1.2], True,
'process'))
while not robot.goals.empty():
robot.move_on_error(multiplier=2.5)
def get_images_poses(robot_samples, cam_name, robot_address, robot_port,
folder_out, file_out):
"""
Gets images from a cam_name and and poses of a UR Robot.
Relies on pre-trained points to direct robot motion. Generates a json file
with the pose of the robot and filename of the corresponding image.
Args:
robot_samples (str): The filename for the file containing the list of
points which the robot should move through. This
file can be generated using the `cb2-record`
command from the ur_cb2 package.
cam_name (str): The name of the cam_name to be used.
Valid options are:
- `flycap`
robot_address (str): The address of the robot in form: `###.###.###`
robot_port (int): The port of the robot
folder_out (str): The folder in which to save the data.
file_out (str): The file in which to save all of the generated data.
"""
with open(robot_samples, 'r') as f:
data = json.load(f)
write_time = data['time']
points = data['points']
print('read in {} points, written at: {}'.format(
len(points.keys()), write_time))
tcp2robot = []
im_num = 0
if not os.path.isdir(folder_out):
os.mkdir(folder_out)
with cb2_robot.URRobot(robot_address, robot_port) as robot:
with camera.Camera(cam_name) as cam:
for number in sorted([int(x) for x in points.keys()]):
robot.add_goal(
cb2_robot.Goal(points[str(number)]['joint'], False,
'joint'))
# TODO: this appears to skip the first point!
robot.move_on_stop()
print('Beginning move: {}'.format(number))
while not (robot.at_goal() and robot.is_stopped()):
time.sleep(.25)
time.sleep(.25) # let everything settle
with robot.receiver.lock:
tcp2robot.append(robot.receiver.position)
cv2.imwrite(os.path.join(folder_out,
str(im_num) + '.png'),
cam.capture_raw())
im_num += 1
print(np.asarray(tcp2robot))
# `[x,y,z,<rotation vector>]` where `<rotation vector>` is a three element
# vector representing the an axis about which to rotate (`<x,y,z>`) in
# radians equal to the magnitude of the vector.
json_dict = {"time": str(datetime.datetime.now()), "tcp2robot": tcp2robot}
with open(
os.path.join(folder_out,
os.path.splitext(file_out)[0] + '.json'), 'w') as \
result_json_file:
json.dump(json_dict, result_json_file, indent=4)
def get_correspondences(robot_samples, calibration, rows, cols, spacing,
camera, robot_address, robot_port, file_out):
"""
Gets correspondences between a camera and UR Robot with a grid attached.
Relies on pre-trained points to direct robot motion. Will try to find the
grid 5 times per position. Generates a json file with all of the data
needed o then calculate the tool offset and the camera to robot
transformation.
Args:
robot_samples (str): The filename for the file containing the list of
points which the robot should move through. This
file can be generated using the `cb2-record`
command from the ur_cb2 package.
calibration (str): The filename of the camera calibration information.
This file can be generated using the
`calibrate-camera` command from the
camera-calibration toolbox.
rows (int): The number of rows on the grid which is attached to the
robot
cols (int): The number of columns on the grid which is attached to the
robot
spacing (float): The spacing in mm between grid corners on the grid
which is attached to the robot
camera (str): The name of the camera to be used.
Valid options are:
- `flycap`
robot_address (str): The address of the robot in form: `###.###.###`
robot_port (int): The port of the robot
file_out (str): The file in which to save all of the generated data.
"""
with open(robot_samples, 'r') as f:
data = json.load(f)
write_time = data['time']
points = data['points']
print('read in {} points, written at: {}'.format(
len(points.keys()), write_time))
camera2grid = []
tcp2robot = []
with ci.GridLocation(calibration, rows, cols, spacing, camera) as calib:
with cb2_robot.URRobot(robot_address, robot_port) as robot:
for number in sorted([int(x) for x in points.keys()]):
robot.add_goal(
cb2_robot.Goal(points[str(number)]['joint'], False,
'joint'))
# TODO: this appears to skip the first point!
robot.move_on_stop()
print('Beginning move: {}'.format(number))
while not (robot.at_goal() and robot.is_stopped()):
time.sleep(.25)
time.sleep(.25) # let everything settle
print("reached goal")
go_on = 0
while go_on <= 5:
try:
camera2grid.append(calib.get_cam2grid())
calib.show_images()
with robot.receiver.lock:
tcp2robot.append(robot.receiver.position)
print("got the grid")
go_on = 6
except RuntimeError as e:
print("something went wrong: {}".format(e))
go_on += 1
tcp2robot = np.array(tcp2robot)
tcp2robot[:, 0:3] = tcp2robot[:, 0:3] * 1000
tcp2robot = tcp2robot.tolist()
print(np.asarray(tcp2robot)) # Axis-Angle [x,y,z,ax,ay,az]
print(np.asarray(camera2grid))
json_dict = {
"grid": {
"rows": rows,
"cols": cols,
"spacing": spacing
},
"time": str(datetime.datetime.now()),
"calibration": calibration,
"tcp2robot": tcp2robot,
"camera2grid": camera2grid
}
with open(os.path.splitext(file_out)[0] + '.json', 'w') as \
result_json_file:
json.dump(json_dict, result_json_file, indent=4)
def semi_track(robot, current):
"""
Start at the center and do a complete circuit motion
Args:
robot:
current:
Returns:
"""
spliner = Spline(order=2)
# A point little to the right of the center
point1 = list(current)
point1[0] = current[0] + 0.2
# a point above the previous point.
# Should form curve path to this using previous point as a control point
point2 = list(point1)
point2[2] = point1[2] + 0.1
path = spliner.get_path(current[:3],
point1[:3],
point2[:3],
n=30,
bezier=True)
#print(path)
for p in path:
robot.add_goal(
cb2_robot.Goal([p[0], p[1], p[2], 1.2, -1.2, 1.2],
True,
'process',
radius=0.001))
# Continue moving slightly up
point3 = list(point2)
point3[2] += 0.01
robot.add_goal(
cb2_robot.Goal([point3[0], point3[1], point3[2], 1.2, -1.2, 1.2],
True,
'process',
radius=0.001))
# A little more up from point2
point4 = list(point2)
point4[2] += 0.05
# A point to the left of point 4.
# Should have a curve path from 3 to 5 via control point 4
point5 = list(point4)
point5[0] -= 0.2
path = spliner.get_path(point3[:3],
point4[:3],
point5[:3],
n=30,
bezier=True)
#print(path)
for p in path:
robot.add_goal(
cb2_robot.Goal([p[0], p[1], p[2], 1.2, -1.2, 1.2],
True,
'process',
radius=0.001))
while not robot.goals.empty():
robot.move_on_error(multiplier=2.5)
def circuit(robot, start):
spliner = Spline(order=2)
# A point little to the right of the start
point1 = list(start)
point1[0] += 0.2
# a point above the previous point.
# Should form curve path to this using previous point as a control point
point2 = list(point1)
point2[2] = point1[2] + 0.1
path = spliner.get_path(start[:3],
point1[:3],
point2[:3],
n=50,
bezier=True)
for p in path:
robot.add_goal(
cb2_robot.Goal([p[0], p[1], p[2], 1.2, -1.2, 1.2],
True,
'process',
radius=0.001))
# Continue moving slightly up
point3 = list(point2)
point3[2] += 0.01
robot.add_goal(
cb2_robot.Goal([point3[0], point3[1], point3[2], 1.2, -1.2, 1.2],
True,
'process',
radius=0.001))
# A little more up from point3
point4 = list(point2)
point4[2] += 0.05
# A point to the left of point 4.
# Should have a curve path from 3 to 5 via control point 4
point5 = list(point4)
point5[0] -= 0.2
path = spliner.get_path(point3[:3],
point4[:3],
point5[:3],
n=50,
bezier=True)
for p in path:
robot.add_goal(
cb2_robot.Goal([p[0], p[1], p[2], 1.2, -1.2, 1.2],
True,
'process',
radius=0.001))
# Point more to the left of point 5 and slightly below
point6 = list(point5)
point6[0] -= 0.3
point6[2] -= 0.1
# point down to point 6
#point7 = list(point6)
#point7[2] -= 0.1
# move back to the start point in a curve
path = spliner.get_path(point5[:3],
point6[:3],
start[:3],
n=50,
bezier=True)
for p in path[:-5]:
robot.add_goal(
cb2_robot.Goal([p[0], p[1], p[2], 1.2, -1.2, 1.2],
True,
'process',
radius=0.001))
while not robot.goals.empty():
robot.move_on_error(multiplier=2.0)
robot.add_goal(
cb2_robot.Goal([path[-1][0], path[-1][1], path[-1][2], 1.2, -1.2, 1.2],
True,
'process',
radius=0.001))
while not robot.goals.empty():
robot.move_on_error(multiplier=1.0)
time.sleep(2)