from frankapy.utils import transform_to_list, min_jerk
from tqdm import trange
import rospy
if __name__ == "__main__":
fa = FrankaArm()
fa.reset_joints()
fa.close_gripper()
while True:
input(
'Presse [Enter] to enter guide mode and move robot to be on top of a flat surface.'
)
fa.run_guide_mode()
while True:
inp = input('[r]etry or [c]ontinue? ')
if inp not in ('r', 'c'):
print('Please give valid input!')
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)
import argparse
from frankapy import FrankaArm
if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument('--time', '-t', type=float, default=100)
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('Applying 0 force torque control for {}s'.format(args.time))
fa.run_guide_mode(args.time)
