def main():
"""
A main loop for user interactions.
Returns
-------
None.
"""
i = 0
ex_con = ("q", "quit", "exit")
t_con = ("t", "test")
gs_con = ("grid", "gs", "grid_search", "opt", "optimise")
sys_con = ("sys", "solve", "s")
help_con = ("h", "help")
pre_con = ("p", "preset", "pre")
print(in_str)
while i==0:
cl()
print(loop_str)
inp = input("Enter what you wish to do: ").lower()
cl()
if inp in ex_con:
i = 1
elif inp in t_con:
print("Testing everything (This may take a while):")
testing.test_all()
elif inp in gs_con:
optimise()
elif inp in sys_con:
run_system()
elif inp in pre_con:
run_presets()
elif inp in help_con:
print(help_str)
else:
print(invalid_str)
input("Press enter to continue.")
brett.head.set_pan_tilt(0,0)
@testme
def test_torso():
brett.torso.go_down()
brett.torso.go_up()
@testme
def test_grippers():
brett.lgrip.close()
brett.lgrip.open()
@testme
def test_base():
brett.base.set_twist(0,1,.1)
if __name__ == "__main__":
if rospy.get_name() == "/unnamed": # for ipython convenience
rospy.init_node("test_pr2",disable_signals=True)
brett = pr2.PR2()
rospy.sleep(.5)
test_all()
#
#brett.larm.get_pose_matrix(
#posemat = brett.larm.get_pose_matrix('torso_lift_link','l_gripper_palm_link')
#brett.larm.goto_pose_matrix(posemat, 'torso_lift_link', 'l_gripper_palm_link')
from ransac import ransac, ConstantModel
import numpy as np
import testing
@testing.testme
def test_ConstantModel():
true_inliers = np.random.rand(10,3)
true_outliers = np.zeros((3,3)) + 100
data = np.concatenate([true_inliers, true_outliers])
true_inlier_inds = np.arange(10,dtype=int)
est_model, est_inlier_inds, est_error = ransac(data, ConstantModel, 1, 10, 1, 5)
assert np.all(est_inlier_inds == true_inlier_inds)
if __name__ == "__main__":
testing.test_all(stop=True)
