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