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)