# grasp object frank.grasp_shape(tag2worldTF, realpeginsert.Circle, monitor_execution=False, use_planner=True, training=False) # fa.run_guide_mode(15) # fa.goto_gripper(0.025, grasp=True) goal = goal2worldTF.translation goal[2] += 0.03 # offset the height goal[0] += 0.01 print('Goal Loc = {}'.format(goal)) current_pose = fa.get_pose().translation print('Current Loc = {}'.format(current_pose)) goal_pose = (goal - current_pose).tolist() print('Sensor Value = {}'.format(goal_pose)) # scaling factor for z trajectory goal_pose.append(-0.5) print(goal_pose) input("Press Enter to continue...") print('Running the dmp') # fa.run_guide_mode(args.time) fa.execute_pose_dmp(pose_dmp_info, duration=args.time, use_goal_formulation=False, initial_sensor_values=goal_pose,
next_position = RigidTransform( rotation=np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]]), translation=np.array(max_location), # add x and z later from_frame='franka_tool', to_frame='world') fa.goto_pose_with_cartesian_control(next_position, 3) starting_location = [constant_x, constant_y, constant_z] print(starting_location) if args.filename is not None: magnetic_calibration.saveData(args.filename + '_x', starting_location, vision_object_location, max_location, fa.get_pose().translation) 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 cropped_image = rgb_cv_image[(object_ycenter - radius):(object_ycenter + radius + 1), (object_xcenter - radius):(object_xcenter + radius + 1)] cv2.imwrite(args.filename + '_x.png', cropped_image)
type=str, default= '/home/stevenl3/Documents/planorparam/scripts/real_robot/april_tag_pick_place_azure_kinect_cfg.yaml' ) parser.add_argument('--no_grasp', '-ng', action='store_true') args = parser.parse_args() cfg = YamlConfig(args.cfg) T_camera_world = RigidTransform.load(cfg['T_k4a_franka_path']) logging.info('Starting robot') fa = FrankaArm() # fa.reset_joints() # fa.reset_pose() # fa.open_gripper() T_ready_world = fa.get_pose() T_ready_world.translation[0] += 0.25 T_ready_world.translation[2] = 0.4 #ret = fa.goto_pose(T_ready_world) logging.info('Init camera') #sensor = get_first_realsense_sensor(cfg['rs']) #original sensor = Kinect2SensorFactory.sensor('bridged', cfg) #Kinect sensor object sensor.start() logging.info('Detecting April Tags') april = AprilTagDetector(cfg['april_tag']) #intr = sensor.color_intrinsics #original
import numpy as np from autolab_core import RigidTransform from frankapy import FrankaArm if __name__ == "__main__": fa = FrankaArm() # reset franka to its home joints fa.reset_joints() # read functions T_ee_world = fa.get_pose() print('Translation: {} | Rotation: {}'.format(T_ee_world.translation, T_ee_world.quaternion)) joints = fa.get_joints() print('Joints: {}'.format(joints)) gripper_width = fa.get_gripper_width() print('Gripper width: {}'.format(gripper_width)) # 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()
filename = '../calibration/2020-01-14 11-08 E1 2X Board Calibration.npz' magnetic_calibration.load_calibration_file(filename) magnetic_calibration.start_recording_data() desired_object_pose = RigidTransform( rotation=np.array([[0, -1, 0], [0, 0, 1], [-1, 0, 0]]), translation=np.array([object_center_point_in_world.x, object_center_point_in_world.y - 0.01, constant_z]), from_frame='franka_tool') fa.goto_pose_with_cartesian_control(desired_object_pose, 10.0) magnetic_calibration.stop_recording_data() max_location = magnetic_calibration.get_max_location() current_position = fa.get_pose().position current_data = magnetic_calibration.get_processed_data() # print("My current data is: " + str(current_data[0]) + ", " + str(current_data[1]) + ", " + str(current_data[2])) # print("My current position is: " + str(current_position[0]) + ", " + str(current_position[1]) + ", " + str(current_position[2])) # print("My next position is: " + str(max_location[0]) + ", " + str(max_location[1]) + ", " + str(max_location[2])) # input("Press enter to continue to command") # franka_arm.move -relative dist m in x and y next_position = RigidTransform(rotation=np.array([ [0, -1, 0], [0, 0, 1], [-1, 0, 0] ]), translation=np.array(max_location), # add x and z later from_frame='franka_tool', to_frame='world') fa.goto_pose_with_cartesian_control(next_position, 3)
from frankapy import FrankaArm if __name__ == '__main__': print('Starting robot') fa = FrankaArm() fa.reset_joints() pose = fa.get_pose() pose.translation[0] = 0.75 # This should trigger an error fa.goto_pose(pose)
from frankapy import FrankaArm import numpy as np fa = FrankaArm() for i in range(20): input("Prepare to move arm") fa.apply_effector_forces_torques(3, 0, 0, 0) rt = fa.get_pose() print(rt)
duration=duration, cartesian_impedances=gains) if open_gripper: fa.open_gripper() if __name__ == '__main__': # Examples and for collecting robot poses # Everything is hard coded :/ print('Starting robot') fa = FrankaArm() go2start(fa) dw = DoorWorld() print(fa.get_pose()) print(fa.get_joints()) input("Press enter to continue") go2door(fa, close_gripper=True, use_planner = True, world=dw) fa.close_gripper() res = turn_knob(fa, use_planner = True, world = dw) if res == "expected_model_failure" or res == "model_deviation": gains1 = [1000,1000,1000,1,1,1] dmp_info2 = "/home/stevenl3/misc_backups/robot_data_door_turn2_position_weights.pkl" fa.execute_position_dmp(dmp_info2, duration=10, skill_desc='position_dmp', cartesian_impedance=gains1) else: print("Worked as expected!")
else: break if inp == 'c': break rospy.loginfo('Generating Trajectory') # EE will follow a 2D circle while pressing down with a target force dt = 0.01 T = 10 ts = np.arange(0, T, dt) N = len(ts) dthetas = np.linspace(-np.pi / 2, 3 * np.pi / 2, N) r = 0.07 circ = r * np.c_[np.sin(dthetas), np.cos(dthetas)] start_pose = fa.get_pose() start_pose.rotation = FC.HOME_POSE.rotation target_poses = [] for i, t in enumerate(ts): pose = start_pose.copy() pose.translation[0] += r + circ[i, 0] pose.translation[1] += circ[i, 1] target_poses.append(pose) target_force = [0, 0, -10, 0, 0, 0] S = [1, 1, 1, 1, 1, 1] position_kps_cart = FC.DEFAULT_TRANSLATIONAL_STIFFNESSES + FC.DEFAULT_ROTATIONAL_STIFFNESSES force_kps_cart = [0.1] * 6 position_kps_joint = FC.DEFAULT_K_GAINS force_kps_joint = [0.1] * 7 rospy.loginfo('Initializing Sensor Publisher')
from frankapy import FrankaArm, SensorDataMessageType from frankapy import FrankaConstants as FC from frankapy.proto_utils import sensor_proto2ros_msg, make_sensor_group_msg from frankapy.proto import PosePositionSensorMessage, ShouldTerminateSensorMessage, CartesianImpedanceSensorMessage from franka_interface_msgs.msg import SensorDataGroup from frankapy.utils import min_jerk, min_jerk_weight import rospy if __name__ == "__main__": fa = FrankaArm() fa.reset_joints() rospy.loginfo('Generating Trajectory') p0 = fa.get_pose() p1 = p0.copy() T_delta = RigidTransform(translation=np.array([0, 0, 0.2]), rotation=RigidTransform.z_axis_rotation( np.deg2rad(30)), from_frame=p1.from_frame, to_frame=p1.from_frame) p1 = p1 * T_delta fa.goto_pose(p1) T = 5 dt = 0.02 ts = np.arange(0, T, dt) weights = [min_jerk_weight(t, T) for t in ts] pose_traj = [p1.interpolate_with(p0, w) for w in weights]
xtranslation_3cm = RigidTransform(rotation=np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]), translation=np.array([0.03, 0, 0]), from_frame='franka_tool', to_frame='world') random_position = RigidTransform( rotation=np.array([[0.9323473, -0.35858258, 0.04612846], [-0.35996283, -0.93259467, 0.02597504], [0.03370496, -0.04082229, -0.99859775]]), translation=np.array([0.39247965, -0.21613652, 0.3882055]), from_frame='franka_tool', to_frame='world') print(fa.get_pose().translation) print(fa.get_joints()) desired_joints_1 = [ -0.52733715, 0.25603565, 0.47721503, -1.26705864, 0.00600359, 1.60788199, 0.63019184 ] desired_joints_2 = [ -0.16017485, 1.12476619, 0.26004398, -0.67246923, 0.04899213, 2.08439578, 0.81627789 ] fa.reset_joints()
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')