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"