def main(): rospy.init_node('testing_node') limb = Limb('right') print('yay') # r = rospy.Rate(1000) # limb.set_command_timeout(.1) wrench = limb.endpoint_effort() force = wrench['force'] mag = force_mag(force) force_2d_dir = normalize( proj(force, normalize([1, 1, 0])) * normalize([force[0], force[1], 0])) rotation_matrix = np.matrix([[np.cos(np.pi / 2), -np.sin(np.pi / 2), 0], [np.sin(np.pi / 2), np.cos(np.pi / 2), 0], [0, 0, 1]]) force_base_frame = rotation_matrix * np.reshape(force_2d_dir, (3, 1)) force_base_frame[0] = -force_base_frame[0] force_base_frame = np.array(force_base_frame).flatten() print("force_base_frame: ", force_base_frame) print("force_base_frame: ", type(force_base_frame)) # print(force_2d_dir) cur_pos = limb.endpoint_pose().get('position') print("force: ", force) print("force 2d dir", type(force_2d_dir))
def calibrate_measurement(): #return the mean of the force noise and standard 3* std of the force noise. force_measurements = [] # torque_measurements = [] limb = Limb() count = 5 while count > 0: wrench = limb.endpoint_effort() force = wrench.get('force') # torque = wrench.get('torque') force_measurements.append(force) # torque_measurements.append(torque) rospy.sleep(0.05) count = count - 1 force_matrix = np.matrix(force_measurements) # torque_matrix = np.matrix(torque_measurements) print("force_matrix: ", force_matrix) force_noise_mean = force_matrix.mean(0) # torque_mean = torque_matrix.mean(0) force_noise_std = force_matrix.std(0) * 3 error = [force_noise_mean[0], force_noise_std[0]] print("error", error) return error