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