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
