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)
def get_object_pos_mean(self, num_measurements=10, timeout=-1):
object_pose_data=array([0.]*7)
init_time=time.time()
counter=0
while (counter<num_measurements):
if time.time()-init_time>timeout:
print "Timeout"
return(array([]))
#for i in xrange(num_measurements):
pose=self.get_object_pos(blocking=False)
if len(pose)==0:
pass
#print "No object data"
else:
init_time=time.time()
object_pose_data[:3]+=pose[:3,3]
object_pose_data[3:]+=quat.from_matrix(pose)
counter+=1
object_pose_data/=num_measurements
object_pose_data[3:]/=norm(object_pose_data[3:])
return(quat.to_matrix_fixed(object_pose_data[3:], r=object_pose_data[:3]))
