pick.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import sys
00004 import rospy
00005 from moveit_commander import RobotCommander, PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
00006 from geometry_msgs.msg import PoseStamped
00007 
00008 if __name__=='__main__':
00009 
00010     roscpp_initialize(sys.argv)
00011     rospy.init_node('moveit_py_demo', anonymous=True)
00012     
00013     scene = PlanningSceneInterface()
00014     robot = RobotCommander()
00015     rospy.sleep(1)
00016 
00017     # clean the scene
00018     scene.remove_world_object("pole")
00019     scene.remove_world_object("table")
00020     scene.remove_world_object("part")
00021 
00022     # publish a demo scene
00023     p = PoseStamped()
00024     p.header.frame_id = robot.get_planning_frame()
00025     p.pose.position.x = 0.7
00026     p.pose.position.y = -0.4
00027     p.pose.position.z = 0.85
00028     p.pose.orientation.w = 1.0
00029     scene.add_box("pole", p, (0.3, 0.1, 1.0))
00030 
00031     p.pose.position.y = -0.2
00032     p.pose.position.z = 0.175
00033     scene.add_box("table", p, (0.5, 1.5, 0.35))
00034 
00035     p.pose.position.x = 0.6
00036     p.pose.position.y = -0.7
00037     p.pose.position.z = 0.5
00038     scene.add_box("part", p, (0.15, 0.1, 0.3))
00039 
00040     rospy.sleep(1)
00041 
00042     # pick an object
00043     robot.right_arm.pick("part")
00044 
00045     rospy.spin()
00046     roscpp_shutdown()


moveit_commander
Author(s): Ioan Sucan
autogenerated on Mon Oct 6 2014 02:24:45