import utils.conversions as conv
from brett2.PR2 import *
#from brett2.PR2 import mirror_arm_joints
from brett2.ghost_PR2 import GhostPR2
from math import pi
import tf
import numpy
action_client = []
if __name__=='__main__':
rospy.init_node('ankush')
bot = PR2()
rospy.loginfo(bot.base.get_pose())
#bot.base.goto_pose(10,5.0,pi/2, '/map')
#bot.torso.go_up()
p = numpy.array([1,0,0])
bot.head.look_at(p, 'base_link', 'narrow_stereo_link')
fold_reset = [pi/32, pi/12, 0, -pi/2, pi , -pi/6, -pi/4]
bot.larm.goto_joint_positions(fold_reset)
bot.rarm.goto_joint_positions(mirror_arm_joints(fold_reset))
rospy.sleep(4)
bot.lgrip.open()
parser.add_argument("arms_used")
parser.add_argument("--dry_run", action="store_true")
args = parser.parse_args()
return args
if __name__ == "__main__":
if rospy.get_name() == "/unnamed":
rospy.init_node("multi_item_teach_verb", disable_signals=True)
args = get_args()
# arguments need to be: <verb> <items separated by ','> <arms_used separated by ',' and corresponding to items>
verb = args.verb
items_str = args.items
items = items_str.split(",")
arms_used = args.arms_used.split(",")
data_dir = osp.join(osp.dirname(lfd.__file__), "data")
pr2 = PR2.PR2()
move_pr2_to_start_pos(pr2)
demo_name = get_new_demo_name(verb, items)
stage_names_for_demo, special_pts = do_teach_verb(demo_name, items, arms_used, data_dir)
new_entry_text = get_new_demo_entry_text(demo_name, stage_names_for_demo, items, arms_used, special_pts)
yn = yes_or_no("save demonstration?")
# add the entry to the verbs yaml file
if yn:
add_new_entry_to_yaml_file(data_dir, new_entry_text)
else:
print "exiting without saving"
