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