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)
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")
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)
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 send_signal2(self, sig):
self.poll()
if self.returncode is None:
dprint("Sending signal ", sig, " to subprocess ", self.args[0])
self.send_signal(sig)
