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')
