def find_force_noise_levels(force_handle, finger, filter_order=3, filter_freq=0.3, measurements=10):
vector_filter=Filter_vector(order=filter_order, freq=filter_freq)
#stabilizing filter
for i in xrange(measurements):
force=force_handle.get_force(finger, update=True)
vector_filter.filter(force)
#real measurement
max_force=array([0.]*3)
max_norm_force=0.
for i in xrange(measurements):
force=force_handle.get_force(finger, update=True)
force_filtered=vector_filter.filter(force)
max_force=npmax(array([max_force, force_filtered]), axis=0)
norm_force=norm(force_filtered)
max_norm_force=max(max_norm_force, norm_force)
return(max_force, max_norm_force)
def approach_until_touch(hand_handle, finger, arm_handle, force_handle, touch_point, force_threshold, goal_precision, speed, start_pose, sim, force_factor, extra_tool=identity(4)):
filters_order=3
filters_freq=0.2
max_noise_force, max_noise_norm_force=find_force_noise_levels(force_handle, finger, filter_order=filters_order, filter_freq=filters_freq, measurements=50)
print "noise levels", max_noise_force, max_noise_norm_force
#if False:
if not sim:
force_threshold=max_noise_norm_force*1.3
#raw_input()
force=array([0.]*3)
init_time=time.time()
old_time=init_time
first=True
force_filter=Filter_vector(dim=3, order=filters_order, freq=filters_freq)
while norm(force)<force_threshold:
cur_time=time.time()
period=cur_time-old_time
old_time=cur_time
data=force_handle.get_data()[finger]
print "data", data, force_factor
force=array(data[-3:])*force_factor
force=force_filter.filter(force)
eprint("FORCE: ", norm(force), "Threshold", force_threshold)
cur_pose=quat.to_matrix_fixed(data[3:7], r=data[:3])
if first:
vel_vector=(touch_point[:2]-start_pose[:2,3])
vel_vector/=norm(vel_vector)
first=False
pose=start_pose
step=vel_vector*speed*period
pose[:2,3]+=step
move_robot(hand_handle, finger, arm_handle, pose, goal_precision, wait=0.1, extra_tool=extra_tool)
print "Pos", cur_pose, "Force", force
#stop robot
dprint("STop movement")
cur_pose=quat.to_matrix_fixed(data[3:7], r=data[:3])
cur_pose[:3,:3]=pose[:3,:3]
move_robot(hand_handle, finger, arm_handle, cur_pose, goal_precision, extra_tool=extra_tool)
