def main(SIM, DEBUG=True):
# PATH SETTING
CONFIG = 'config.yaml'
with open(CONFIG) as f:
path = yaml.load(f)
APose_GOAL = path['ROOT'] + 'goal/ap_goal.yaml'
CCalibration_GOAL = path['ROOT'] + 'goal/cc_goal.yaml'
Robot = hardward_controller.MoveGroupInteface()
Camera = hardward_controller.camera_shooter()
try:
# AutoCenter(Robot, Camera, path, SIM, DEBUG)
# AutoFocus(Robot, Camera, path, SIM, DEBUG)
# AutoPose(Robot, Camera, path, SIM, DEBUG)
# CameraPoseEstimation(Robot, Camera, path, SIM, DEBUG)
# SolveXZ(Robot, Camera, path, DEBUG)
HoleSearching(Robot, Camera, path, SIM, DEBUG)
PegInHole(Robot, Camera, path, SIM, DEBUG)
print("============ Calibration process complete!")
except rospy.ROSInterruptException:
return
except KeyboardInterrupt:
return
yaml.dump(ret.tolist(), f, default_flow_style=False)
with open(TIME_PATH, 'w') as f:
yaml.dump(time.tolist(), f, default_flow_style=False)
t = []
JSposition = []
print('Data grabbed with series number: %s' % (test_time))
if __name__ == "__main__":
# PATH SETTING
CONFIG = 'config.yaml'
with open(CONFIG) as f:
path = yaml.load(f)
Robot = hardward_controller.MoveGroupInteface()
js_sub = rospy.Subscriber('/vs6242/joint_states', JointState, JScallback)
rosGoal_B_star = [0.227013667126, 0.204820632069, 0.191043148954, -3.02239056684e-05, -0.999999994061, 6.35861126952e-05, 8.31888538146e-05]
rosGoal_B_init = [0.276983683655, 0.165763650009, 0.239330956866, 0.0861362693318, 0.981339693465, 0.100947450405, 0.139149421103]
B_init = as_MatrixGoal(rosGoal_B_init)
B_star = as_MatrixGoal(rosGoal_B_star)
B_tildes = getIdealPiHCommand(B_star)
Bc = np.array([[[-1.0000 , 0.0008 , -0.0007 , 0.2020],
[0.0008 , 1.0000 , 0.0007 , 0.1747],
[0.0007 , 0.0007 , -1.0000 , 0.1610],
[ 0 , 0 , 0 , 1.0000]]])
test_holeID = [0]