예제 #1
0
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)
예제 #2
0
파일: utils.py 프로젝트: arcoslab/cmoc
 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]))