def main():
# Argument Parsing
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt, description=main.__doc__)
parser.add_argument(
'-v', '--verbose',
action='store_const',
const=True,
default=False,
help="displays debug information (default = False)"
)
parser.add_argument(
'-l', '--limb',
choices=['left', 'right'],
#required=True,
default='left',
help='the limb to run the measurements on'
)
args = parser.parse_args(rospy.myargv()[1:])
#Init
rospy.init_node("pen_and_paper", anonymous = True)
time.sleep(0.5)
print("Init started...")
arm = arm_class.Arm(args.limb, args.verbose, True)
print("Init finished...")
arm.set_neutral(False)
raw_input("Press Enter to grab pen...")
for i in range(3):
print("gripping in: {}".format(3-i))
time.sleep(1)
arm._gripper.close()
#high pose
if not arm.move_to_pose(arm_class.alter_pose_inc(pose, args.verbose, posz=0.12)):
arm.simple_failsafe()
return False
raw_input("mark point")
if not arm.move_direct(pose):
arm.simple_failsafe()
return False
#draw point
if not arm.move_to_pose(arm_class.alter_pose_inc(pose, args.verbose, posz=-0.01)):
arm.simple_failsafe()
return False
print(arm._current_pose)
for i in range(20):
if not arm.move_to_pose(arm_class.alter_pose_inc(pose, args.verbose, posz=(i*0.02))):
arm.simple_failsafe()
return False
time.sleep(1)
arm.cam.update_snapshot = True
print(arm._current_pose)
raw_input("{} - ".format(i+1))
#exit strategy
arm.set_neutral(False)
raw_input("please remove pen...")
arm.simple_failsafe(True)
def measure_positive_x(arm, step_width=0.01, speed=0.3):
"""
Approaches one point 20 times and measures the difference between given pose and reached pose.
The approaches contain two movements. The first is to minimize joint play, whereas the second is to reach the measurement pose.
Parameter:
arm: The robots limb to perform the motion
step_width: Width of the steps to be made in meter; Default: 0.01
speed: Speed at which the arm shall move; Default: 0.3; Range: 0.0-1.0
Return:
diff_dict: Dictionary containing the measured data. Structure: diff_dict['x' : list(), 'y' : list()]
"""
print("--- {} arm: positive x").format(arm._limb_name)
arm._limb.set_joint_position_speed(speed)
diff_dict = dict()
diff_dict['x'] = list()
diff_dict['y'] = list()
for round_counter in range(20):
#Minimize joint play
if not arm.move_to_pose(
arm_class.alter_pose_inc(start_pose[arm._limb_name].pose,
arm._verbose,
posx=(step_width * -2))):
arm.simple_failsafe()
return False
#Initial pose
initial_pose = deepcopy(start_pose[arm._limb_name].pose)
if not arm.move_to_pose(
arm_class.alter_pose_inc(
initial_pose, verbose=arm._verbose, posx=-(step_width))):
arm.simple_failsafe()
return False
print("--->Reached: initial pose\nStarting Measurements...")
#Measuring
next_pose = arm_class.alter_pose_inc(
arm_class.alter_pose_inc(initial_pose,
arm._verbose,
posx=step_width))
if not arm.move_to_pose(next_pose):
arm.simple_failsafe()
return False
diff_dict['x'].append(arm._current_pose.position.x -
next_pose.position.x)
diff_dict['y'].append(arm._current_pose.position.y -
next_pose.position.y)
return diff_dict
def positive_x(arm, step_width, workspace_x=0.297, workspace_y=0.210):
"""
Measure the accuracy of the given robots limb in a workspace of given size.
The number of measurement points depends on the relationship between workspace size and step width.
Parameter:
arm: The robots limb to be measured on
step_width: Distance between the measurement points
workspace_x: Size of the workspace along the x axis
workspace_y: Size of the workspace along the y axis
Return:
out_dict: Dictionary that contains the measured data; Structure: outdict['x{}y{}' = Point]
"""
print("--- {} arm: positive x").format(arm._limb_name)
#raw_input("Press Enter to start...")
out_dict = dict()
if step_width < 0.02:
#Initial Pose
if arm.get_solution(
arm_class.alter_pose_inc(deepcopy(
start_pose[arm._limb_name].pose),
arm._verbose,
posx=-(step_width * 2))
): #approach starting point with minimal joint play
arm.move_to_solution()
else:
arm.simple_failsafe()
return False
initial_pose = deepcopy(start_pose[arm._limb_name].pose)
if arm.get_solution(
arm_class.alter_pose_inc(initial_pose,
verbose=arm._verbose,
posx=-(step_width))):
arm.move_to_solution()
else:
arm.simple_failsafe()
return False
print("--->Reached: initial pose\nStarting Measurements...")
#Measuring
next_pose = deepcopy(initial_pose)
for y_step in arange(initial_pose.position.y,
initial_pose.position.y + workspace_y,
step_width):
next_pose = arm_class.alter_pose_abs(next_pose,
verbose=arm._verbose,
posy=y_step)
for x_step in arange(initial_pose.position.x,
initial_pose.position.x + workspace_x,
step_width):
next_pose = arm_class.alter_pose_abs(next_pose,
verbose=arm._verbose,
posx=x_step)
if arm.get_solution(next_pose):
arm.move_to_solution()
else:
arm.simple_failsafe()
return False
diff = Point(
x=arm._current_pose.position.x - next_pose.position.x,
y=arm._current_pose.position.y - next_pose.position.y,
z=arm._current_pose.position.z - next_pose.position.z)
out_dict['x{}y{}'.format(
((int)(next_pose.position.x * 100)),
((int)(next_pose.position.y * 100)))] = diff
else:
next_pose = deepcopy(start_pose[arm._limb_name].pose)
for y_step in arange(
start_pose[arm._limb_name].pose.position.y,
start_pose[arm._limb_name].pose.position.y + workspace_y,
step_width):
next_pose = arm_class.alter_pose_abs(next_pose,
verbose=arm._verbose,
posy=y_step)
for x_step in arange(
start_pose[arm._limb_name].pose.position.x,
start_pose[arm._limb_name].pose.position.x + workspace_x,
step_width):
next_pose = arm_class.alter_pose_abs(next_pose,
verbose=arm._verbose,
posx=x_step)
if not arm.move_precise(next_pose):
arm.simple_failsafe()
return False
diff = Point(
x=arm._current_pose.position.x - next_pose.position.x,
y=arm._current_pose.position.y - next_pose.position.y,
z=arm._current_pose.position.z - next_pose.position.z)
out_dict['y{}x{}'.format(
((int)(next_pose.position.y * 100)),
((int)(next_pose.position.x * 100)))] = diff
print("row {} finished".format(y_step))
#Print data
print("Pose,x_diff[mm],y_diff[mm],z_diff[mm],,")
for k in out_dict.keys():
print("{},{},{},{},,").format(k, out_dict[k].x * 1000,
out_dict[k].y * 1000,
out_dict[k].z * 1000)
return out_dict
def improve_pose(pose, lut, limb_name='left'):
"""
Applies the data of the given LUT-Object to the given pose to improve its accuracy.
Parameter:
pose: Pose to be improved
lut: LUT to improve pose with
limb_name: Name of the robots limb on which the LUT got created
Return:
pose: Improved pose
"""
#Create possibilities
y_name = "y{}".format(int(pose.position.y * 100))
y_name_plus = "y{}".format(int(pose.position.y * 100) + 1)
y_name_minus = "y{}".format(int(pose.position.y * 100) - 1)
x_name = "x{}".format(int(pose.position.x * 100))
x_name_plus = "x{}".format(int(pose.position.x * 100) + 1)
x_name_minus = "x{}".format(int(pose.position.x * 100) - 1)
#poses y resembles point in LUT
if y_name in lut[limb_name]['x_pos'].keys():
#poses x resembles point in LUT
if x_name in lut[limb_name]['x_pos'][y_name].keys():
x_diff = lut[limb_name]['x_pos'][y_name][x_name].x
y_diff = lut[limb_name]['x_pos'][y_name][x_name].y
z_diff = lut[limb_name]['x_pos'][y_name][x_name].z
#poses x slightly smaller than point in LUT
elif x_name_plus in lut[limb_name]['x_pos'][y_name].keys():
x_name_minus_2 = "x{}".format(int(pose.position.x * 100) - 2)
x_diff = ((lut[limb_name]['x_pos'][y_name][x_name_plus].x * 2) +
lut[limb_name]['x_pos'][y_name][x_name_minus_2].x) / 3
y_diff = ((lut[limb_name]['x_pos'][y_name][x_name_plus].y * 2) +
lut[limb_name]['x_pos'][y_name][x_name_minus_2].y) / 3
z_diff = ((lut[limb_name]['x_pos'][y_name][x_name_plus].z * 2) +
lut[limb_name]['x_pos'][y_name][x_name_minus_2].z) / 3
#poses x slightly bigger than point in LUT
elif x_name_minus in lut[limb_name]['x_pos'][y_name].keys():
x_name_plus_2 = "x{}".format(int(pose.position.x * 100) + 2)
x_diff = ((lut[limb_name]['x_pos'][y_name][x_name_minus].x * 2) +
lut[limb_name]['x_pos'][y_name][x_name_plus_2].x) / 3
y_diff = ((lut[limb_name]['x_pos'][y_name][x_name_minus].y * 2) +
lut[limb_name]['x_pos'][y_name][x_name_plus_2].y) / 3
z_diff = ((lut[limb_name]['x_pos'][y_name][x_name_minus].z * 2) +
lut[limb_name]['x_pos'][y_name][x_name_plus_2].z) / 3
else:
print("improve_pose: given pose not in improvable workspace")
return pose
#poses y slightly smaller than point in LUT
elif y_name_plus in lut[limb_name]['x_pos'].keys():
#poses x resembles point in LUT
if x_name in lut[limb_name]['x_pos'][y_name_plus].keys():
x_diff = lut[limb_name]['x_pos'][y_name_plus][x_name].x
y_diff = lut[limb_name]['x_pos'][y_name_plus][x_name].y
z_diff = lut[limb_name]['x_pos'][y_name_plus][x_name].z
#poses x slightly smaller than point in LUT
elif x_name_plus in lut[limb_name]['x_pos'][y_name_plus].keys():
x_name_minus_2 = "x{}".format(int(pose.position.x * 100) - 2)
x_diff = (
(lut[limb_name]['x_pos'][y_name_plus][x_name_plus].x * 2) +
lut[limb_name]['x_pos'][y_name_plus][x_name_minus_2].x) / 3
y_diff = (
(lut[limb_name]['x_pos'][y_name_plus][x_name_plus].y * 2) +
lut[limb_name]['x_pos'][y_name_plus][x_name_minus_2].y) / 3
z_diff = (
(lut[limb_name]['x_pos'][y_name_plus][x_name_plus].z * 2) +
lut[limb_name]['x_pos'][y_name_plus][x_name_minus_2].z) / 3
#poses x slightly bigger than point in LUT
elif x_name_minus in lut[limb_name]['x_pos'][y_name_plus].keys():
x_name_plus_2 = "x{}".format(int(pose.position.x * 100) + 2)
x_diff = (
(lut[limb_name]['x_pos'][y_name_plus][x_name_minus].x * 2) +
lut[limb_name]['x_pos'][y_name_plus][x_name_plus_2].x) / 3
y_diff = (
(lut[limb_name]['x_pos'][y_name_plus][x_name_minus].y * 2) +
lut[limb_name]['x_pos'][y_name_plus][x_name_plus_2].y) / 3
z_diff = (
(lut[limb_name]['x_pos'][y_name_plus][x_name_minus].z * 2) +
lut[limb_name]['x_pos'][y_name_plus][x_name_plus_2].z) / 3
else:
print("improve_pose: given pose not in improvable workspace")
return pose
#Given poses y is slightly bigger than point in the LUT
elif y_name_minus in lut[limb_name]['x_pos'].keys():
#poses x resembles point in LUT
if x_name in lut[limb_name]['x_pos'][y_name_minus].keys():
x_diff = lut[limb_name]['x_pos'][y_name_minus][x_name].x
y_diff = lut[limb_name]['x_pos'][y_name_minus][x_name].y
z_diff = lut[limb_name]['x_pos'][y_name_minus][x_name].z
#poses x slightly smaller than point in LUT
elif x_name_plus in lut[limb_name]['x_pos'][y_name_minus].keys():
x_name_minus_2 = "x{}".format(int(pose.position.x * 100) - 2)
x_diff = (
(lut[limb_name]['x_pos'][y_name_minus][x_name_plus].x * 2) +
lut[limb_name]['x_pos'][y_name_minus][x_name_minus_2].x) / 3
y_diff = (
(lut[limb_name]['x_pos'][y_name_minus][x_name_plus].y * 2) +
lut[limb_name]['x_pos'][y_name_minus][x_name_minus_2].y) / 3
z_diff = (
(lut[limb_name]['x_pos'][y_name_minus][x_name_plus].z * 2) +
lut[limb_name]['x_pos'][y_name_minus][x_name_minus_2].z) / 3
#poses x slightly bigger than point in LUT
elif x_name_minus in lut[limb_name]['x_pos'][y_name_minus].keys():
x_name_plus_2 = "x{}".format(int(pose.position.x * 100) + 2)
x_diff = (
(lut[limb_name]['x_pos'][y_name_minus][x_name_minus].x * 2) +
lut[limb_name]['x_pos'][y_name_minus][x_name_plus_2].x) / 3
y_diff = (
(lut[limb_name]['x_pos'][y_name_minus][x_name_minus].y * 2) +
lut[limb_name]['x_pos'][y_name_minus][x_name_plus_2].y) / 3
z_diff = (
(lut[limb_name]['x_pos'][y_name_minus][x_name_minus].z * 2) +
lut[limb_name]['x_pos'][y_name_minus][x_name_plus_2].z) / 3
else:
print("improve_pose: given pose not in improvable workspace")
return pose
else:
print("improve_pose: given pose not in improvable workspace")
return pose
return arm_class.alter_pose_inc(pose,
posx=(-x_diff),
posy=(-y_diff),
posz=(-z_diff))
def positive_x(arm, rounds):
"""
Executes ten movements with a positive increment of 2cm along the x axis and measures the difference between expected pose and reached pose.
This motion gets repeated for a number of given rounds.
Parameter:
arm: The robots limb to be measured on
rounds: Number of rounds to be executed
Return:
True/False: False if any movement is not executable, else True
"""
print("--- {} arm: positive x").format(arm._limb_name)
raw_input("Press Enter to start...")
for round_counter in range(rounds):
#Minimize joint play
if arm.get_solution(
arm_class.alter_pose_inc(deepcopy(
middle_pose[arm._limb_name].pose),
arm._verbose,
posx=-0.12)):
arm.move_to_solution()
else:
arm.simple_failsafe()
return False
#Initial Pose
initial_pose = deepcopy(middle_pose[arm._limb_name].pose)
if arm.get_solution(
arm_class.alter_pose_inc(initial_pose,
verbose=arm._verbose,
posx=-0.1)):
arm.move_to_solution()
else:
arm.simple_failsafe()
return False
print("--->Reached: initial pose\nStarting Measurements...")
#Readings: ['posenumber', 'setpoint', 'actual', 'difference in x', 'difference in y']
setpoint = list()
actual = list()
x_diff = list()
y_diff = list()
#Measuring
for x in range(1, 11):
next_pose = arm_class.alter_pose_inc(
arm_class.alter_pose_inc(deepcopy(initial_pose),
arm._verbose,
posx=x * 0.02))
if arm.get_solution(next_pose):
arm.move_to_solution()
else:
arm.simple_failsafe()
return False
print("Reached Pose {}").format((int)(x))
setpoint.append(next_pose.position.x)
actual.append(arm._current_pose.position.x)
x_diff.append(arm._current_pose.position.x - next_pose.position.x)
y_diff.append(arm._current_pose.position.y - next_pose.position.y)
print("Finished: {} arm: positive x").format(arm._limb_name)
print("Data following: round {}\n").format(round_counter)
for step_counter in range(10):
print("{},{},{},{},{}").format(step_counter + 1,
setpoint[step_counter],
actual[step_counter],
x_diff[step_counter],
y_diff[step_counter])
print("")
return True
def main():
try:
# Argument Parsing
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__)
parser.add_argument(
'-v',
'--verbose',
action='store_const',
const=True,
default=False,
help="displays debug information (default = False)")
""" parser.add_argument(
'-l', '--limb',
choices=['left', 'right'],
required=True,
#default='left',
help='the limb to run the measurements on'
) """
args = parser.parse_args(rospy.myargv()[1:])
#Init
rospy.init_node("measure_precision", anonymous=True)
time.sleep(0.5)
print("--- Ctrl-D stops the program ---")
print("Init started...")
arm = arm_class.Arm('left', verbose=args.verbose)
lut = lut_interface.restore_lut_from_file(
"/home/user/schuermann_BA/ros_ws/src/baxter_staples/precision/lut.csv"
)['lut']
number_of_rounds = 10
filelocation = "/home/user/schuermann_BA/ros_ws/src/baxter_staples/precision/"
test_poses = [
arm_class.alter_pose_inc(deepcopy(start_pose['left'].pose),
verbose=args.verbose,
posx=0.06,
posy=0.06),
arm_class.alter_pose_inc(deepcopy(start_pose['left'].pose),
verbose=args.verbose,
posx=0.06,
posy=0.15),
arm_class.alter_pose_inc(deepcopy(start_pose['left'].pose),
verbose=args.verbose,
posx=0.14,
posy=0.11),
arm_class.alter_pose_inc(deepcopy(start_pose['left'].pose),
verbose=args.verbose,
posx=0.23,
posy=0.06),
arm_class.alter_pose_inc(deepcopy(start_pose['left'].pose),
verbose=args.verbose,
posx=0.23,
posy=0.15)
]
print("Init finished...")
#Measurements
for n in range(number_of_rounds):
print(n)
for p in range(len(test_poses)):
#not improved measurements
if arm.get_solution(test_poses[p]):
arm.move_to_solution()
time.sleep(0.05)
save_data(
arm, test_poses[p],
filelocation + "not_improved_{}.csv".format(p + 1))
time.sleep(0.05)
else:
arm.simple_failsafe()
sys.exit()
for p in range(len(test_poses)):
#improved with move_precise
if arm.move_precise(test_poses[p]):
time.sleep(0.05)
save_data(
arm, test_poses[p],
filelocation + "move_precise_{}.csv".format(p + 1))
time.sleep(0.05)
else:
arm.simple_failsafe()
sys.exit()
for p in range(len(test_poses)):
#improved with lut
if arm.get_solution(
lut_interface.improve_pose(deepcopy(test_poses[p]),
lut=lut,
limb_name=arm._limb_name)):
arm.move_to_solution()
time.sleep(0.05)
save_data(arm, test_poses[p],
filelocation + "with_lut_{}.csv".format(p + 1))
time.sleep(0.05)
else:
arm.simple_failsafe()
sys.exit()
for p in range(len(test_poses)):
#improved with lut an move_precise
if arm.move_precise(
lut_interface.improve_pose(deepcopy(test_poses[p]),
lut=lut,
limb_name=arm._limb_name)):
time.sleep(0.05)
save_data(arm, test_poses[p],
filelocation + "all_{}.csv".format(p + 1))
time.sleep(0.05)
else:
arm.simple_failsafe()
sys.exit()
print("finished measurements")
except rospy.ROSInterruptException as e:
return e
except KeyboardInterrupt as e:
return e