points_world = T_camera_world * intr.deproject(depth_im) if cfg['vis_detect']: vis3d.figure() vis3d.pose(RigidTransform()) vis3d.points(subsample(points_world.data.T), color=(0, 1, 0), scale=0.002) vis3d.pose(T_ready_world, length=0.05) vis3d.pose(T_camera_world, length=0.1) vis3d.pose(T_tag_world) vis3d.pose(T_grasp_world) vis3d.pose(T_lift_world) vis3d.show() if not args.no_grasp: logging.info('Commanding robot') fa.goto_pose_with_cartesian_control(T_lift_world) fa.goto_pose_with_cartesian_control(T_grasp_world) fa.close_gripper() fa.goto_pose_with_cartesian_control(T_lift_world) sleep(3) fa.goto_pose_with_cartesian_control(T_grasp_world) fa.open_gripper() fa.goto_pose_with_cartesian_control(T_lift_world) fa.goto_pose_with_cartesian_control(T_ready_world) import IPython IPython.embed() exit(0)
777.7421875, height=1536, width=2048) T_tag_camera = april.detect(sensor, intr, vis=cfg['vis_detect'])[0] T_tag_world = T_camera_world * T_tag_camera logging.info('Tag has translation {}'.format(T_tag_world.translation)) import ipdb ipdb.set_trace() T_tag_tool = RigidTransform(rotation=np.eye(3), translation=[0, 0, 0.04], from_frame=T_tag_world.from_frame, to_frame="franka_tool") T_tool_world = T_tag_world * T_tag_tool.inverse() fa.goto_pose_with_cartesian_control( T_tool_world, cartesian_impedances=[2000, 2000, 1000, 300, 300, 300]) logging.info('Finding closest orthogonal grasp') T_grasp_world = get_closest_grasp_pose(T_tag_world, T_ready_world) T_lift = RigidTransform(translation=[0, 0, 0.2], from_frame=T_ready_world.to_frame, to_frame=T_ready_world.to_frame) T_lift_world = T_lift * T_grasp_world logging.info('Visualizing poses') _, depth_im, _ = sensor.frames() points_world = T_camera_world * intr.deproject(depth_im) if cfg['vis_detect']: vis3d.figure() vis3d.pose(RigidTransform())
x_scan_dist = -0.05 else: x_scan_dist = 0.05 if constant_z > 0.087: z_scan_dist = -0.02 else: z_scan_dist = 0.02 desired_pose = RigidTransform(rotation=np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]]), translation=np.array( [constant_x, constant_y, constant_z]), from_frame='franka_tool', to_frame='world') fa.goto_pose_with_cartesian_control(desired_pose, 10.0) desired_object_pose = RigidTransform(rotation=np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]]), translation=np.array([ constant_x + x_scan_dist, constant_y, constant_z ]), from_frame='franka_tool', to_frame='world') magnetic_calibration = MagneticCalibration() filename = '../calibration/2020-01-14 11-08 E1 2X Board Calibration.npz' magnetic_calibration.load_calibration_file(filename)
from frankapy import FrankaArm import numpy as np from autolab_core import RigidTransform fa = FrankaArm() trans, rot = np.load("above_pose.npy") #fa.close_gripper() above_position = RigidTransform(rotation=rot, translation=trans, from_frame='franka_tool', to_frame='world') fa.goto_pose_with_cartesian_control( above_position, cartesian_impedances=[3000, 3000, 100, 300, 300, 300]) fa.open_gripper() input("Add gripper") fa.close_gripper() trans, rot = np.load("insert_pose.npy") insert_position = RigidTransform(rotation=rot, translation=trans, from_frame='franka_tool', to_frame='world') insert_position.translation[-1] -= 0.01 # manual adjustment fa.goto_pose_with_cartesian_control( insert_position, cartesian_impedances=[20, 20, 2000, 300, 300, 300] ) #one of these but with impedance control? compliance comes from the matrix so I think that's good enough
def run_main(): parser = argparse.ArgumentParser() parser.add_argument('--filename', type=str, default=None) args = parser.parse_args() fa = FrankaArm() fa.open_gripper() # fa.reset_pose() fa.reset_joints(10) initial_magnet_position1 = RigidTransform( rotation=np.array([[0, -1, 0], [0, 0, 1], [-1, 0, 0]]), translation=np.array([0.38592997, 0.10820438, 0.08264024]), from_frame='franka_tool', to_frame='world') initial_magnet_position2 = RigidTransform( rotation=np.array([[0, -1, 0], [0, 0, 1], [-1, 0, 0]]), translation=np.array([0.38592997, 0.20820438, 0.08264024]), from_frame='franka_tool', to_frame='world') squeeze_position1 = RigidTransform( rotation=np.array([[0, 1, 0], [0, 0, 1], [1, 0, 0]]), translation=np.array([0.54900978, 0.20820438, 0.20654183]), from_frame='franka_tool', to_frame='world') squeeze_position2 = RigidTransform( rotation=np.array([[0, 1, 0], [0, 0, 1], [1, 0, 0]]), translation=np.array([0.54900978, 0.20820438, 0.15654183]), from_frame='franka_tool', to_frame='world') relative_pos_dist_z = RigidTransform(rotation=np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]), translation=np.array([0.0, 0.0, 0.1]), from_frame='franka_tool', to_frame='franka_tool') relative_pos_dist_y = RigidTransform(rotation=np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]), translation=np.array([0.0, 0.1, 0.0]), from_frame='franka_tool', to_frame='franka_tool') relative_rotation_z = RigidTransform(rotation=np.array([[0, 1, 0], [-1, 0, 0], [0, 0, 1]]), translation=np.array([0.0, 0.0, 0.0]), from_frame='franka_tool', to_frame='franka_tool') relative_neg_rotation_z = RigidTransform(rotation=np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]]), translation=np.array( [0.0, 0.0, 0.0]), from_frame='franka_tool', to_frame='franka_tool') relative_neg_dist_y = RigidTransform(rotation=np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]), translation=np.array([0.0, -0.1, 0.0]), from_frame='franka_tool', to_frame='franka_tool') relative_neg_dist_z = RigidTransform(rotation=np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]), translation=np.array([0.0, 0.0, -0.1]), from_frame='franka_tool', to_frame='franka_tool') relative_pos_dist_x = RigidTransform( rotation=np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]), translation=np.array([0.075, 0.0, 0.0]), from_frame='franka_tool', to_frame='franka_tool') relative_neg_dist_x = RigidTransform( rotation=np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]), translation=np.array([-0.03, 0.0, 0.0]), from_frame='franka_tool', to_frame='franka_tool') fa.goto_pose_with_cartesian_control(initial_magnet_position1) fa.goto_pose_with_cartesian_control(initial_magnet_position2) magnetic_calibration = MagneticCalibration() filename = '../calibration/2020-01-14 11-08 E1 2X Board Calibration.npz' magnetic_calibration.load_calibration_file(filename) # Close the gripper and save corresponding robot state, gripper state, and magnetic state data magnetic_calibration.start_recording_data() time.sleep(1) # fa.goto_pose_delta_with_cartesian_control(relative_x_dist, 10) # max width is about 0.080 m gripper_step_size = 0.002 # in meters num_samples = 30 noise_level = 13 # uT force_threshold = 1.05 GRIPPER_CONTACT = False min_contact = None min_gripper_width = None global_min = None current_force_estimate = 1 while (GRIPPER_CONTACT == False): current_width = magnetic_calibration.get_current_gripper_width() print(current_width) fa.goto_gripper(current_width - gripper_step_size) #magnetic_calibration.get_last_magnetic_data() mag_data = magnetic_calibration.get_previous_magnetic_samples( num_samples) # grab last ten samples xyz_mag_data = mag_data[:, 6] print(xyz_mag_data) #slope = np.diff(xyz_mag_data, axis=0) #print(slope) # asign = np.sign(slope) # signchange = ((np.roll(asign, 1) - asign) != 0).astype(int) # print(signchange) # first make sure the signal changes are large enough to not just be noise jitter # then see if there is a signal change in slope - this signifies a contact. #if(np.any(abs(np.diff(mag_data)>noise_level))): #if(abs(np.sum(np.sign(np.diff(mag_data)))<numSamples) & abs(np.diff(mag_data))>noise_level): if global_min is None: global_min = np.min(xyz_mag_data, axis=0) elif global_min > np.min(xyz_mag_data, axis=0): global_min = np.min(xyz_mag_data, axis=0) if xyz_mag_data[-1] - global_min > noise_level: GRIPPER_CONTACT = True min_gripper_width = current_width - 2 * gripper_step_size print(min_gripper_width) print("Min Magnetic Values: ") print(min_contact) gripper_step_size -= 0.00003 # if (np.any(np.logical_and((abs(slope)>noise_level), (slope > 0)))): # # if last value is negative, contact. if last value is positive, release. # # save max value so we can compare current value and strength of grip # GRIPPER_CONTACT = True # min_contact = np.min(xyz_mag_data, axis=0) # min_gripper_width = current_width-gripper_step_size # print(min_gripper_width) # print("Min Magnetic Values: ") # print(min_contact) #input("Press enter to continue to next step") #magnetic_calibration.close_gripper_magnetic_feedback() # current_width = min_gripper_width # num_samples = 100 # while(current_force_estimate < force_threshold): # current_width -= gripper_step_size # print(current_width) # fa.goto_gripper(current_width) # mag_data = magnetic_calibration.get_previous_magnetic_samples(num_samples) # grab last ten samples # z_mag_data = np.max(mag_data[:,6]) # print(z_mag_data) # current_force_estimate = global_min / z_mag_data # print(current_force_estimate) fa.goto_pose_delta_with_cartesian_control(relative_pos_dist_z) current_joints = fa.get_joints() current_joints[6] -= math.pi fa.goto_joints(list(current_joints)) current_position = fa.get_pose() fa.goto_pose_with_cartesian_control(squeeze_position1) fa.goto_pose_with_cartesian_control(squeeze_position2) for i in range(3): for j in range(i + 1): fa.goto_gripper(0.0, force=20) fa.goto_gripper(min_gripper_width) if i < 2: fa.goto_pose_delta_with_cartesian_control(relative_pos_dist_x) fa.goto_pose_with_cartesian_control(current_position, 5) current_joints = fa.get_joints() current_joints[6] += math.pi fa.goto_joints(list(current_joints)) fa.goto_pose_delta_with_cartesian_control(relative_neg_dist_z) fa.open_gripper() fa.goto_pose_with_cartesian_control(initial_magnet_position1) #fa.goto_pose_delta_with_cartesian_control(relative_neg_dist_z) magnetic_calibration.stop_recording_data() #fa.open_gripper() print(magnetic_calibration.magnetic_data[0, :]) if args.filename is not None: magnetic_calibration.saveData(args.filename, min_contact, min_gripper_width) magnetic_calibration.plotData(magnetic_calibration.magnetic_data, 'Raw Data Over Time') magnetic_calibration.plotGripperData( magnetic_calibration.gripper_state_data, 'Raw Data Over Time') magnetic_calibration.plotGripperData(magnetic_calibration.robot_state_data, 'Raw Data Over Time')
def run_main(): parser = argparse.ArgumentParser() parser.add_argument('--filename', type=str, default=None) args = parser.parse_args() fa = FrankaArm() fa.open_gripper() # fa.reset_pose() fa.reset_joints(10) pipette_rotation = np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]]) initial_magnet_position1 = RigidTransform( rotation=pipette_rotation, translation=np.array([0.45683638, 0.06513334, 0.20451204]), from_frame='franka_tool', to_frame='world') initial_magnet_position2 = RigidTransform( rotation=pipette_rotation, translation=np.array([0.45683638, 0.06513334, 0.15451204]), from_frame='franka_tool', to_frame='world') initial_magnet_position3 = RigidTransform( rotation=pipette_rotation, translation=np.array([0.45683638, 0.06513334, 0.33451204]), from_frame='franka_tool', to_frame='world') beaker_position1 = RigidTransform(rotation=pipette_rotation, translation=np.array( [0.44724006, 0.2870516, 0.33210899]), from_frame='franka_tool', to_frame='world') beaker_position2 = RigidTransform(rotation=pipette_rotation, translation=np.array( [0.44724006, 0.2870516, 0.16210899]), from_frame='franka_tool', to_frame='world') test_tube_1 = RigidTransform(rotation=pipette_rotation, translation=np.array( [0.54359048, 0.17224207, 0.19106597]), from_frame='franka_tool', to_frame='world') test_tube_2 = RigidTransform(rotation=pipette_rotation, translation=np.array( [0.56359048, 0.17224207, 0.19106597]), from_frame='franka_tool', to_frame='world') test_tube_3 = RigidTransform(rotation=pipette_rotation, translation=np.array( [0.58359048, 0.17224207, 0.19106597]), from_frame='franka_tool', to_frame='world') relative_pos_dist_z = RigidTransform( rotation=np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]), translation=np.array([0.0, 0.0, 0.175]), from_frame='franka_tool', to_frame='franka_tool') relative_pos_dist_y = RigidTransform(rotation=np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]), translation=np.array([0.0, 0.02, 0.0]), from_frame='franka_tool', to_frame='franka_tool') relative_rotation_z = RigidTransform(rotation=np.array([[0, 1, 0], [-1, 0, 0], [0, 0, 1]]), translation=np.array([0.0, 0.0, 0.0]), from_frame='franka_tool', to_frame='franka_tool') relative_neg_rotation_z = RigidTransform(rotation=np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]]), translation=np.array( [0.0, 0.0, 0.0]), from_frame='franka_tool', to_frame='franka_tool') relative_neg_dist_y = RigidTransform(rotation=np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]), translation=np.array([0.0, -0.1, 0.0]), from_frame='franka_tool', to_frame='franka_tool') relative_neg_dist_z = RigidTransform( rotation=np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]), translation=np.array([0.0, 0.0, -0.175]), from_frame='franka_tool', to_frame='franka_tool') relative_pos_dist_x = RigidTransform( rotation=np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]), translation=np.array([0.075, 0.0, 0.0]), from_frame='franka_tool', to_frame='franka_tool') relative_neg_dist_x = RigidTransform( rotation=np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]), translation=np.array([-0.03, 0.0, 0.0]), from_frame='franka_tool', to_frame='franka_tool') fa.goto_pose_with_cartesian_control(initial_magnet_position1) fa.goto_pose_with_cartesian_control(initial_magnet_position2) magnetic_calibration = MagneticCalibration() filename = '../calibration/2020-01-14 11-08 E1 2X Board Calibration.npz' magnetic_calibration.load_calibration_file(filename) # Close the gripper and save corresponding robot state, gripper state, and magnetic state data magnetic_calibration.start_recording_data() time.sleep(1) # fa.goto_pose_delta_with_cartesian_control(relative_x_dist, 10) # max width is about 0.080 m gripper_step_size = 0.005 # in meters num_samples = 30 noise_level = 15 # uT force_threshold = 1.05 GRIPPER_CONTACT = False min_contact = None min_gripper_width = None global_max = None current_force_estimate = 1 while (GRIPPER_CONTACT == False): current_width = magnetic_calibration.get_current_gripper_width() print(current_width) fa.goto_gripper(current_width - gripper_step_size) #magnetic_calibration.get_last_magnetic_data() mag_data = magnetic_calibration.get_previous_magnetic_samples( num_samples) # grab last ten samples xyz_mag_data = mag_data[:, 6] print(xyz_mag_data) #slope = np.diff(xyz_mag_data, axis=0) #print(slope) # asign = np.sign(slope) # signchange = ((np.roll(asign, 1) - asign) != 0).astype(int) # print(signchange) # first make sure the signal changes are large enough to not just be noise jitter # then see if there is a signal change in slope - this signifies a contact. #if(np.any(abs(np.diff(mag_data)>noise_level))): #if(abs(np.sum(np.sign(np.diff(mag_data)))<numSamples) & abs(np.diff(mag_data))>noise_level): if global_max is None: global_max = np.max(xyz_mag_data, axis=0) elif global_max < np.max(xyz_mag_data, axis=0): global_max = np.max(xyz_mag_data, axis=0) if global_max - xyz_mag_data[-1] > noise_level: GRIPPER_CONTACT = True min_gripper_width = current_width fa.goto_gripper(min_gripper_width) #min_contact = xyz_mag_data[-1] print(min_gripper_width) print("Global Max: ") print(global_max) gripper_step_size -= 0.0001 # if (np.any(np.logical_and((abs(slope)>noise_level), (slope > 0)))): # # if last value is negative, contact. if last value is positive, release. # # save max value so we can compare current value and strength of grip # GRIPPER_CONTACT = True # min_contact = np.min(xyz_mag_data, axis=0) # min_gripper_width = current_width-gripper_step_size # print(min_gripper_width) # print("Min Magnetic Values: ") # print(min_contact) #input("Press enter to continue to next step") #magnetic_calibration.close_gripper_magnetic_feedback() # current_width = min_gripper_width # num_samples = 100 # while(current_force_estimate < force_threshold): # current_width -= gripper_step_size # print(current_width) # fa.goto_gripper(current_width) # mag_data = magnetic_calibration.get_previous_magnetic_samples(num_samples) # grab last ten samples # z_mag_data = np.max(mag_data[:,6]) # print(z_mag_data) # current_force_estimate = global_min / z_mag_data # print(current_force_estimate) fa.goto_pose_delta_with_cartesian_control(relative_pos_dist_z) for i in range(3): fa.goto_pose_with_cartesian_control(beaker_position1) fa.goto_pose_with_cartesian_control(beaker_position2) fa.goto_gripper(0.0, force=20) fa.goto_gripper(min_gripper_width) fa.goto_pose_delta_with_cartesian_control(relative_pos_dist_z) if i == 0: force_value = 15 fa.goto_pose_with_cartesian_control(test_tube_1) elif i == 1: force_value = 30 fa.goto_pose_with_cartesian_control(test_tube_2) elif i == 2: force_value = 50 fa.goto_pose_with_cartesian_control(test_tube_3) force_gripper_width = min_gripper_width for j in range(5): mag_data = magnetic_calibration.get_previous_magnetic_samples( num_samples) max_data = np.max(mag_data[:, 6], axis=0) current_force_estimate = max_data - force_value print("Force Threshold:") print(current_force_estimate) force_achieved = False gripper_step_size = 0.0003 num_samples = 30 while (force_achieved == False): force_gripper_width -= gripper_step_size fa.goto_gripper(force_gripper_width) #magnetic_calibration.get_last_magnetic_data() mag_data = magnetic_calibration.get_previous_magnetic_samples( num_samples) # grab last ten samples print(np.mean(mag_data[:, 6])) #slope = np.diff(xyz_mag_data, axis=0) #print(slope) # asign = np.sign(slope) # signchange = ((np.roll(asign, 1) - asign) != 0).astype(int) # print(signchange) # first make sure the signal changes are large enough to not just be noise jitter # then see if there is a signal change in slope - this signifies a contact. #if(np.any(abs(np.diff(mag_data)>noise_level))): #if(abs(np.sum(np.sign(np.diff(mag_data)))<numSamples) & abs(np.diff(mag_data))>noise_level): if np.mean(mag_data[:, 6]) < current_force_estimate: force_achieved = True print(force_gripper_width) if (j < 4): fa.goto_pose_delta_with_cartesian_control(relative_pos_dist_y) fa.goto_gripper(min_gripper_width) fa.goto_pose_delta_with_cartesian_control(relative_pos_dist_z) # for i in range(5): # fa.goto_pose_with_cartesian_control(beaker_position1) # fa.goto_pose_with_cartesian_control(beaker_position2) # fa.goto_gripper(0.0, force = 20) # fa.goto_gripper(min_gripper_width) # fa.goto_pose_delta_with_cartesian_control(relative_pos_dist_z) # #fa.goto_gripper(min_gripper_width) # fa.goto_pose_with_cartesian_control(test_tube_1) # fa.goto_gripper(force_gripper_width) # fa.goto_gripper(min_gripper_width) fa.goto_pose_with_cartesian_control(beaker_position1) fa.goto_pose_with_cartesian_control(beaker_position2) fa.goto_gripper(0.0, force=20) fa.goto_pose_delta_with_cartesian_control(relative_pos_dist_z) fa.goto_gripper(min_gripper_width) fa.goto_pose_with_cartesian_control(initial_magnet_position3) fa.goto_pose_with_cartesian_control(initial_magnet_position1) fa.goto_pose_with_cartesian_control(initial_magnet_position2) fa.open_gripper() fa.goto_pose_with_cartesian_control(initial_magnet_position1) # for i in range(3): # for j in range(i+1): # fa.goto_gripper(0.0, force = 20) # fa.goto_gripper(min_gripper_width) # if i < 2: # fa.goto_pose_delta_with_cartesian_control(relative_pos_dist_x) # fa.goto_pose_with_cartesian_control(current_position,5) # current_joints = fa.get_joints() # current_joints[6] += math.pi # fa.goto_joints(list(current_joints)) # fa.goto_pose_delta_with_cartesian_control(relative_neg_dist_z) # fa.open_gripper() # fa.goto_pose_with_cartesian_control(initial_magnet_position1) #fa.goto_pose_delta_with_cartesian_control(relative_neg_dist_z) magnetic_calibration.stop_recording_data() #fa.open_gripper() print(magnetic_calibration.magnetic_data[0, :]) if args.filename is not None: magnetic_calibration.saveData(args.filename, global_max, min_gripper_width) magnetic_calibration.plotData(magnetic_calibration.magnetic_data, 'Raw Data Over Time') magnetic_calibration.plotGripperData( magnetic_calibration.gripper_state_data, 'Raw Data Over Time') magnetic_calibration.plotGripperData(magnetic_calibration.robot_state_data, 'Raw Data Over Time')
def run_main(): parser = argparse.ArgumentParser() parser.add_argument('--filename', type=str, default=None) args = parser.parse_args() fa = FrankaArm(async_cmds=True) fa.close_gripper() while fa.is_skill_done() == False: time.sleep(1) # fa.reset_pose() # fa.reset_joints() drawer_2_z_height = 0.12919677 drawer_3_z_height = 0.08264024 drawer_height = drawer_3_z_height initial_magnet_position1 = RigidTransform( rotation=np.array([[0, -1, 0], [0, 0, 1], [-1, 0, 0]]), translation=np.array([0.40926815, 0.20738414, drawer_height]), from_frame='franka_tool', to_frame='world') initial_magnet_position2 = RigidTransform( rotation=np.array([[0, -1, 0], [0, 0, 1], [-1, 0, 0]]), translation=np.array([0.40926815, 0.23738414, drawer_height]), from_frame='franka_tool', to_frame='world') relative_pos_dist_z = RigidTransform(rotation=np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]), translation=np.array([0.0, 0.0, 0.08]), from_frame='franka_tool', to_frame='franka_tool') relative_pos_dist_y = RigidTransform(rotation=np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]), translation=np.array([0.0, 0.08, 0.0]), from_frame='franka_tool', to_frame='franka_tool') relative_rotation_z = RigidTransform(rotation=np.array([[0, 1, 0], [-1, 0, 0], [0, 0, 1]]), translation=np.array([0.0, 0.0, 0.0]), from_frame='franka_tool', to_frame='franka_tool') relative_neg_rotation_z = RigidTransform(rotation=np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]]), translation=np.array( [0.0, 0.0, 0.0]), from_frame='franka_tool', to_frame='franka_tool') relative_neg_dist_y = RigidTransform( rotation=np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]), translation=np.array([0.0, -0.13, 0.0]), from_frame='franka_tool', to_frame='franka_tool') relative_neg_dist_z = RigidTransform( rotation=np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]), translation=np.array([0.0, 0.0, -0.08]), from_frame='franka_tool', to_frame='franka_tool') relative_pos_dist_x = RigidTransform(rotation=np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]), translation=np.array([0.03, 0.0, 0.0]), from_frame='franka_tool', to_frame='franka_tool') relative_neg_dist_x = RigidTransform( rotation=np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]), translation=np.array([-0.03, 0.0, 0.0]), from_frame='franka_tool', to_frame='franka_tool') fa.goto_pose_with_cartesian_control(initial_magnet_position1) while fa.is_skill_done() == False: time.sleep(1) fa.goto_pose_with_cartesian_control(initial_magnet_position2) while fa.is_skill_done() == False: time.sleep(1) magnetic_calibration = MagneticCalibration() filename = '../calibration/2020-01-14 11-08 E1 2X Board Calibration.npz' magnetic_calibration.load_calibration_file(filename) num_samples = 10 noise_level = 1 # uT #fa.goto_gripper(0.0, force=160) # while fa.is_skill_done() == False: # time.sleep(1) magnetic_calibration.start_recording_data() time.sleep(0.5) greater_than_noise = False fa.goto_pose_delta_with_cartesian_control( relative_pos_dist_y, 5, stop_on_contact_forces=[20, 2, 20, 20, 20, 20]) while fa.is_skill_done() == False: mag_data = magnetic_calibration.get_previous_magnetic_samples( num_samples) dif_mag_data = np.diff(mag_data[:, 5]) mean_change = np.mean(dif_mag_data) print(mean_change) if (abs(mean_change) > noise_level) and not greater_than_noise: greater_than_noise = True if greater_than_noise and abs(mean_change) < 1: print("stopped skill") fa.stop_skill() time.sleep(0.01) # while fa.is_skill_done() == False: # time.sleep(1) fa.goto_pose_delta_with_cartesian_control(relative_neg_dist_y, 5) while fa.is_skill_done() == False: time.sleep(1) magnetic_calibration.stop_recording_data() #fa.open_gripper() while fa.is_skill_done() == False: time.sleep(1) print(magnetic_calibration.magnetic_data[0, :]) if args.filename is not None: magnetic_calibration.saveData(args.filename) magnetic_calibration.plotDataVsRobotState( magnetic_calibration.magnetic_data, magnetic_calibration.robot_state_data, 'Raw Data Over Distance') magnetic_calibration.plotData(magnetic_calibration.magnetic_data, 'Raw Data Over Time') magnetic_calibration.plotForceData(magnetic_calibration.force_state_data, 'Raw Data Over Time') magnetic_calibration.plotRobotStateData( magnetic_calibration.robot_state_data, 'Raw Data Over Time')