Ejemplo n.º 1
0
 def term_wait_kill(self, sec):
     self.poll()
     if self.returncode is None:
         dprint(
             "Sending signal ", signal.SIGTERM, " to subprocess ",
             self.args[0])
         self.send_signal(signal.SIGTERM)
         self.wait_and_kill(sec)
Ejemplo n.º 2
0
 def start(self):
     for process_args in self.processes_args:
         filename=which(process_args[0])
         if filename and os.path.exists(filename) and os.access(filename, os.X_OK):
             dprint("Starting process: ", process_args[0])
             self.processes.append(MyPopen([filename]+process_args[1:]))
             self.processes[-1].args=process_args
         else:
             eprint("Executable", process_args[0], " not found, not starting process")
Ejemplo n.º 3
0
    def update_cur_torques(self,torques,factors=True):
        #This function is called automatically by the hand object
        #here is where the torque adjustment takes place.
        #self.cur_torques=array([torques[0]*self.base_torque_factor,torques[1]*self.proximal_torque_factor,torques[2]*self.distal_torque_factor])
#        self.cur_torques=array([torques[0]*self.base_torque_factor,torques[1]*self.proximal_torque_factor*0.72,torques[2]*self.distal_torque_factor])#0.62
# factors: 1.3,1.3,0.8 look fine
        if factors:
            dprint("With factors", self.torque_calibration_factors)
            self.cur_torques=array(torques)*self.torque_calibration_factors
            #self.cur_torques=array(torques)
        else:
            self.cur_torques=array(torques)
Ejemplo n.º 4
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)
Ejemplo n.º 5
0
 def send_signal2(self, sig):
     self.poll()
     if self.returncode is None:
         dprint("Sending signal ", sig, " to subprocess ", self.args[0])
         self.send_signal(sig)