target_poses[t]), force=target_force) fb_ctrlr_proto = ForcePositionControllerSensorMessage( id=i, timestamp=timestamp, position_kps_cart=position_kps_cart, force_kps_cart=force_kps_cart, selection=S) ros_msg = make_sensor_group_msg( trajectory_generator_sensor_msg=sensor_proto2ros_msg( traj_gen_proto_msg, SensorDataMessageType.FORCE_POSITION), feedback_controller_sensor_msg=sensor_proto2ros_msg( fb_ctrlr_proto, SensorDataMessageType.FORCE_POSITION_GAINS)) pub.publish(ros_msg) rate.sleep() fa.stop_skill() rospy.loginfo('Publishing HFPC trajectory w/ joint gains...') fa.run_dynamic_force_position(duration=T * n_times, buffer_time=3, S=S, use_cartesian_gains=False, position_kps_joint=position_kps_joint, force_kps_joint=force_kps_joint) init_time = rospy.Time.now().to_time() for i in trange(N * n_times): t = i % N timestamp = rospy.Time.now().to_time() - init_time traj_gen_proto_msg = ForcePositionSensorMessage(id=i, timestamp=timestamp, seg_run_time=dt,
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')