if __name__ == '__main__': parser = argparse.ArgumentParser() parser.add_argument('--time', '-t', type=float, default=10) parser.add_argument('--open_gripper', '-o', action='store_true') args = parser.parse_args() print('Starting robot') fa = FrankaArm() if args.open_gripper: fa.open_gripper() print( 'Be very careful!! Make sure the robot can safely move to HOME JOINTS Position.' ) wait_for_enter() fa.reset_joints() print('Using default joint impedances to move back and forth.') wait_for_enter() fa.goto_joints(FC.READY_JOINTS, joint_impedances=FC.DEFAULT_JOINT_IMPEDANCES) fa.goto_joints(FC.HOME_JOINTS) print('Now using different joint impedances to move back and forth.') wait_for_enter() fa.goto_joints(FC.READY_JOINTS, joint_impedances=[1500, 1500, 1500, 1250, 1250, 1000, 1000]) fa.goto_joints(FC.HOME_JOINTS) print('Remember to reset the joint_impedances to defaults.') fa.goto_joints(FC.HOME_JOINTS, joint_impedances=FC.DEFAULT_JOINT_IMPEDANCES)
# gripper controls print('Closing gripper') fa.close_gripper() print('Opening gripper to a specified position') fa.goto_gripper(0.02) print('Opening gripper all the way') fa.open_gripper() # joint controls print('Rotating last joint') joints = fa.get_joints() joints[6] += np.deg2rad(45) fa.goto_joints(joints) joints[6] -= np.deg2rad(45) fa.goto_joints(joints) # end-effector pose control print('Translation') T_ee_world = fa.get_pose() T_ee_world.translation += [0.1, 0, 0.1] fa.goto_pose(T_ee_world) T_ee_world.translation -= [0.1, 0, 0.1] fa.goto_pose(T_ee_world) print('Rotation in end-effector frame') T_ee_rot = RigidTransform( rotation=RigidTransform.x_axis_rotation(np.deg2rad(45)), from_frame='franka_tool', to_frame='franka_tool'
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')
translation=np.array([0.0, 0.05, 0.0]), from_frame='franka_tool', to_frame='franka_tool') fa.goto_pose_delta_with_cartesian_control( relative_pos_dist_y, 3, stop_on_contact_forces=[20, 20, 20, 20, 20, 20]) input("Press enter to continue to command") current_joints = fa.get_joints() current_joints[6] += math.pi fa.goto_joints(list(current_joints)) current_joints = fa.get_joints() current_joints[6] -= math.pi fa.goto_joints(list(current_joints)) fa.open_gripper() rgb_image_msg = rospy.wait_for_message('/rgb/image_raw', Image) try: rgb_cv_image = cv_bridge.imgmsg_to_cv2(rgb_image_msg) except CvBridgeError as e: print(e) radius = 50
joints_1 = fa.get_joints() T = 5 dt = 0.02 ts = np.arange(0, T, dt) joints_traj = [min_jerk(joints_1, joints_0, t, T) for t in ts] rospy.loginfo('Initializing Sensor Publisher') pub = rospy.Publisher(FC.DEFAULT_SENSOR_PUBLISHER_TOPIC, SensorDataGroup, queue_size=1000) rate = rospy.Rate(1 / dt) rospy.loginfo('Publishing joints trajectory...') # To ensure skill doesn't end before completing trajectory, make the buffer time much longer than needed fa.goto_joints(joints_traj[1], duration=T, dynamic=True, buffer_time=10) init_time = rospy.Time.now().to_time() for i in range(2, len(ts)): traj_gen_proto_msg = JointPositionSensorMessage( id=i, timestamp=rospy.Time.now().to_time() - init_time, joints=joints_traj[i]) ros_msg = make_sensor_group_msg( trajectory_generator_sensor_msg=sensor_proto2ros_msg( traj_gen_proto_msg, SensorDataMessageType.JOINT_POSITION)) rospy.loginfo('Publishing: ID {}'.format(traj_gen_proto_msg.id)) pub.publish(ros_msg) rate.sleep() # Stop the skill