def run_main():
fa = FrankaArm()
fa.reset_pose()
fa.reset_joints()
magnetic_calibration = MagneticCalibration()
fa.apply_effector_forces_torques(45, 0, 0, 0)
magnetic_calibration.calibrate()
gains = [2000, 2000, 2000, 1500, 1500, 1000, 1000] #joint impedances
# load the dmp weights
if not args.pose_dmp_weights_path:
args.pose_dmp_weights_path = './data/dmp/IROS2020_dmp_data_goal_position_weights.pkl'
pose_dmp_file = open(args.pose_dmp_weights_path, "rb")
pose_dmp_info = pickle.load(pose_dmp_file)
print('Starting robot')
fa = FrankaArm()
# Reset robot pose and joints
print('Opening Grippers')
fa.goto_gripper(0.08, grasp=False)
print('Reset with pose')
fa.reset_pose(duration=10)
print('Reset with joints')
fa.reset_joints()
# instantiate object for AR tag detection
frank = realpeginsert.Robot()
# get transforms
tag2worldTF = frank.detect_ar_world_pos(straighten=True,
shape_class=shapes.Circle,
goal=False)
print('Object Transform: (x,y) = {}'.format(tag2worldTF))
goal2worldTF = frank.detect_ar_world_pos(straighten=True,
shape_class=shapes.Circle,
goal=True)
# rospy.init_node("Test azure kinect calibration")
print('Starting robot')
fa = FrankaArm()
cv_bridge = CvBridge()
ir_intrinsics = CameraIntrinsics.load(args.intrinsics_file_path)
kinect2_left_to_world_transform = RigidTransform.load(
args.transform_file_path)
# print('Opening Grippers')
# fa.open_gripper()
print('Reset with pose')
fa.reset_pose()
print('Reset with joints')
fa.reset_joints()
fa.close_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)
def onMouse(event, x, y, flags, param):
if event == cv2.EVENT_LBUTTONDOWN:
# draw circle here (etc...)
# rospy.init_node("Test azure kinect calibration")
print('Starting robot')
fa = FrankaArm()
cv_bridge = CvBridge()
ir_intrinsics = CameraIntrinsics.load(args.intrinsics_file_path)
kinect2_overhead_to_world_transform = RigidTransform.load(
args.transform_file_path)
print('Opening Grippers')
fa.open_gripper()
print('Reset with pose')
fa.reset_pose(10)
print('Reset with joints')
fa.reset_joints()
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)
def onMouse(event, x, y, flags, param):
if event == cv2.EVENT_LBUTTONDOWN:
# draw circle here (etc...)
print('x = %d, y = %d' % (x, y))
