class CollisionSceneExample(object):
def __init__(self):
self._scene = PlanningSceneInterface()
# clear the scene
self._scene.remove_world_object()
self.robot = RobotCommander()
# pause to wait for rviz to load
print "============ Waiting while RVIZ displays the scene with obstacles..."
# TODO: need to replace this sleep by explicitly waiting for the scene to be updated.
rospy.sleep(2)
def add_pap1(self):
floor_pose = [1.0, -1.0, -0.17, 0, 0, 0, 1]
floor_dimensions = [5.0, 4.0, 0.02]
wall1_pose = [1.0, 1.0, 0.92, 0, 0, 0, 1]
wall1_dimensions = [5.0, 0.02, 2.18]
wall2_pose = [3.5, -1.0, 0.92, 0, 0, 0, 1]
wall2_dimensions = [0.02, 4.0, 2.18]
wall3_pose = [-1.5, -1.0, 1.0, 0, 0, 0, 1]
wall3_dimensions = [0.02, 4.0, 2.18]
obj1_pose = [0.3, 0.4, -0.08, 0, 0, 0, 1]
obj1_dimensions = [0.27, 0.4, 0.12]
obj2_pose = [0.3, -0.01, -0.08, 0, 0, 0, 1]
obj2_dimensions = [0.27, 0.4, 0.12]
obj3_pose = [0.6, 0.4, -0.08, 0, 0, 0, 1]
obj3_dimensions = [0.27, 0.4, 0.12]
obj4_pose = [0.6, -0.01, -0.08, 0, 0, 0, 1]
obj4_dimensions = [0.27, 0.4, 0.12]
obj5_pose = [0.45, 0.10, 0.05, 0, 0, 0, 1]
obj5_dimensions = [0.27, 0.4, 0.12]
con_bottom_pose = [1.9, -0.5, 0.012, 0, 0, 0, 1]
con_bottom_dimensions = [0.704, 0.80, 0.02]
con_left_pose = [1.9, -0.9, 0.705, 0, 0, 0, 1]
con_left_dimensions = [0.744, 0.044, 1.386]
con_right_pose = [2.252, -0.5, 0.705, 0, 0, 0, 1]
con_right_dimensions = [0.044, 0.8, 1.386]
con_back_pose = [1.548, -0.5, 0.705, 0, 0, 0, 1]
con_back_dimensions = [0.044, 0.8, 1.386]
self.add_box_object("wall1", wall1_dimensions, wall1_pose)
self.add_box_object("floor", floor_dimensions, floor_pose)
self.add_box_object("wall3", wall3_dimensions, wall3_pose)
self.add_box_object("wall2", wall2_dimensions, wall2_pose)
self.add_box_object("obj3", obj3_dimensions, obj3_pose)
self.add_box_object("obj2", obj2_dimensions, obj2_pose)
self.add_box_object("obj1", obj1_dimensions, obj1_pose)
self.add_box_object("obj4", obj4_dimensions, obj4_pose)
self.add_box_object("obj5", obj5_dimensions, obj5_pose)
self.add_box_object("con_bottom", con_bottom_dimensions,
con_bottom_pose)
self.add_box_object("con_left", con_left_dimensions, con_left_pose)
self.add_box_object("con_right", con_right_dimensions, con_right_pose)
self.add_box_object("con_back", con_back_dimensions, con_back_pose)
print "========== Added test 1 scene!!"
def add_pap2(self):
floor_pose = [1.0, -1.0, -0.17, 0, 0, 0, 1]
floor_dimensions = [5.0, 4.0, 0.02]
wall1_pose = [1.0, 1.0, 0.92, 0, 0, 0, 1]
wall1_dimensions = [5.0, 0.02, 2.18]
wall2_pose = [3.5, -1.0, 0.92, 0, 0, 0, 1]
wall2_dimensions = [0.02, 4.0, 2.18]
wall3_pose = [-1.5, -1.0, 1.0, 0, 0, 0, 1]
wall3_dimensions = [0.02, 4.0, 2.18]
obj1_pose = [0.3, 0.4, -0.08, 0, 0, 0, 1]
obj1_dimensions = [0.27, 0.4, 0.12]
obj2_pose = [0.3, -0.01, -0.08, 0, 0, 0, 1]
obj2_dimensions = [0.27, 0.4, 0.12]
obj3_pose = [0.6, 0.4, -0.08, 0, 0, 0, 1]
obj3_dimensions = [0.27, 0.4, 0.12]
obj4_pose = [0.6, -0.01, -0.08, 0, 0, 0, 1]
obj4_dimensions = [0.27, 0.4, 0.12]
obj5_pose = [0.45, 0.10, 0.05, 0, 0, 0, 1]
obj5_dimensions = [0.27, 0.4, 0.12]
obs_pose = [1.7, 0.20, 0.35, 0, 0, 0, 1]
obs_dimensions = [0.1, 0.1, 0.9]
con_bottom_pose = [1.9, -0.5, 0.012, 0, 0, 0, 1]
con_bottom_dimensions = [0.704, 0.80, 0.02]
con_left_pose = [1.9, -0.9, 0.705, 0, 0, 0, 1]
con_left_dimensions = [0.744, 0.044, 1.386]
con_right_pose = [2.252, -0.5, 0.705, 0, 0, 0, 1]
con_right_dimensions = [0.044, 0.8, 1.386]
con_back_pose = [1.548, -0.5, 0.705, 0, 0, 0, 1]
con_back_dimensions = [0.044, 0.8, 1.386]
self.add_box_object("wall1", wall1_dimensions, wall1_pose)
self.add_box_object("floor", floor_dimensions, floor_pose)
self.add_box_object("wall3", wall3_dimensions, wall3_pose)
self.add_box_object("wall2", wall2_dimensions, wall2_pose)
self.add_box_object("obj3", obj3_dimensions, obj3_pose)
self.add_box_object("obj2", obj2_dimensions, obj2_pose)
self.add_box_object("obj1", obj1_dimensions, obj1_pose)
self.add_box_object("obj4", obj4_dimensions, obj4_pose)
self.add_box_object("obj5", obj5_dimensions, obj5_pose)
self.add_box_object("con_bottom", con_bottom_dimensions,
con_bottom_pose)
self.add_box_object("con_left", con_left_dimensions, con_left_pose)
self.add_box_object("con_right", con_right_dimensions, con_right_pose)
self.add_box_object("con_back", con_back_dimensions, con_back_pose)
self.add_box_object("obs", obs_dimensions, obs_pose)
print "========== Added test 2 scene!!"
def add_pap3(self):
floor_pose = [1.0, -1.0, -0.17, 0, 0, 0, 1]
floor_dimensions = [5.0, 4.0, 0.02]
wall1_pose = [1.0, 1.0, 0.92, 0, 0, 0, 1]
wall1_dimensions = [5.0, 0.02, 2.18]
wall2_pose = [3.5, -1.0, 0.92, 0, 0, 0, 1]
wall2_dimensions = [0.02, 4.0, 2.18]
wall3_pose = [-1.5, -1.0, 0.92, 0, 0, 0, 1]
wall3_dimensions = [0.02, 4.0, 2.18]
obj1_pose = [0.3, 0.6, -0.08, 0, 0, 0, 1]
obj1_dimensions = [0.27, 0.4, 0.12]
obj2_pose = [0.3, 0.21, -0.08, 0, 0, 0, 1]
obj2_dimensions = [0.27, 0.4, 0.12]
obj3_pose = [0.6, 0.6, -0.08, 0, 0, 0, 1]
obj3_dimensions = [0.27, 0.4, 0.12]
obj4_pose = [0.6, 0.21, -0.08, 0, 0, 0, 1]
obj4_dimensions = [0.27, 0.4, 0.12]
obj5_pose = [0.45, 0.40, 0.05, 0, 0, 0, 1]
obj5_dimensions = [0.27, 0.4, 0.12]
con_bottom_pose = [1.9, -0.5, 0.012, 0, 0, 0, 1]
con_bottom_dimensions = [0.704, 0.80, 0.02]
con_left_pose = [1.9, -0.9, 0.705, 0, 0, 0, 1]
con_left_dimensions = [0.744, 0.044, 1.386]
con_right_pose = [2.252, -0.5, 0.705, 0, 0, 0, 1]
con_right_dimensions = [0.044, 0.8, 1.386]
con_back_pose = [1.548, -0.5, 0.705, 0, 0, 0, 1]
con_back_dimensions = [0.044, 0.8, 1.386]
con2_bottom_pose = [0.3, -0.5, 0.012, 0, 0, 0, 1]
con2_bottom_dimensions = [0.704, 0.80, 0.02]
con2_left_pose = [0.3, -0.9, 0.705, 0, 0, 0, 1]
con2_left_dimensions = [0.744, 0.044, 1.386]
con2_right_pose = [0.652, -0.5, 0.705, 0, 0, 0, 1]
con2_right_dimensions = [0.044, 0.8, 1.386]
con2_back_pose = [-0.052, -0.5, 0.705, 0, 0, 0, 1]
con2_back_dimensions = [0.044, 0.8, 1.386]
con3_bottom_pose = [1.1, -0.5, 0.012, 0, 0, 0, 1]
con3_bottom_dimensions = [0.704, 0.80, 0.02]
con3_left_pose = [1.1, -0.9, 0.705, 0, 0, 0, 1]
con3_left_dimensions = [0.744, 0.044, 1.386]
con3_right_pose = [1.452, -0.5, 0.705, 0, 0, 0, 1]
con3_right_dimensions = [0.044, 0.8, 1.386]
con3_back_pose = [0.748, -0.5, 0.705, 0, 0, 0, 1]
con3_back_dimensions = [0.044, 0.8, 1.386]
self.add_box_object("wall1", wall1_dimensions, wall1_pose)
self.add_box_object("floor", floor_dimensions, floor_pose)
self.add_box_object("wall3", wall3_dimensions, wall3_pose)
self.add_box_object("wall2", wall2_dimensions, wall2_pose)
self.add_box_object("obj3", obj3_dimensions, obj3_pose)
self.add_box_object("obj2", obj2_dimensions, obj2_pose)
self.add_box_object("obj1", obj1_dimensions, obj1_pose)
self.add_box_object("obj4", obj4_dimensions, obj4_pose)
self.add_box_object("obj5", obj5_dimensions, obj5_pose)
self.add_box_object("con_bottom", con_bottom_dimensions,
con_bottom_pose)
self.add_box_object("con_left", con_left_dimensions, con_left_pose)
self.add_box_object("con_right", con_right_dimensions, con_right_pose)
self.add_box_object("con_back", con_back_dimensions, con_back_pose)
self.add_box_object("con2_bottom", con2_bottom_dimensions,
con2_bottom_pose)
self.add_box_object("con2_left", con2_left_dimensions, con2_left_pose)
self.add_box_object("con2_right", con2_right_dimensions,
con2_right_pose)
self.add_box_object("con2_back", con2_back_dimensions, con2_back_pose)
self.add_box_object("con3_bottom", con3_bottom_dimensions,
con3_bottom_pose)
self.add_box_object("con3_left", con3_left_dimensions, con3_left_pose)
self.add_box_object("con3_right", con3_right_dimensions,
con3_right_pose)
self.add_box_object("con3_back", con3_back_dimensions, con3_back_pose)
print "========== Added test 3 scene!!"
def add_pap4(self):
floor_pose = [1.0, -1.0, -0.17, 0, 0, 0, 1]
floor_dimensions = [5.0, 4.0, 0.02]
wall1_pose = [1.0, 1.0, 0.92, 0, 0, 0, 1]
wall1_dimensions = [5.0, 0.02, 2.18]
wall2_pose = [3.5, -1.0, 0.92, 0, 0, 0, 1]
wall2_dimensions = [0.02, 4.0, 2.18]
wall3_pose = [-1.5, -1.0, 0.92, 0, 0, 0, 1]
wall3_dimensions = [0.02, 4.0, 2.18]
obj1_pose = [0.3, 0.6, -0.08, 0, 0, 0, 1]
obj1_dimensions = [0.27, 0.4, 0.12]
obj2_pose = [0.3, 0.21, -0.08, 0, 0, 0, 1]
obj2_dimensions = [0.27, 0.4, 0.12]
obj3_pose = [0.6, 0.6, -0.08, 0, 0, 0, 1]
obj3_dimensions = [0.27, 0.4, 0.12]
obj4_pose = [0.6, 0.21, -0.08, 0, 0, 0, 1]
obj4_dimensions = [0.27, 0.4, 0.12]
obj5_pose = [0.45, 0.40, 0.05, 0, 0, 0, 1]
obj5_dimensions = [0.27, 0.4, 0.12]
obj10_pose = [2.08, -0.65, 0.1, 0, 0, 0, 1]
obj10_dimensions = [0.27, 0.4, 0.12]
obj11_pose = [2.08, -0.65, 0.23, 0, 0, 0, 1]
obj11_dimensions = [0.27, 0.4, 0.12]
obj12_pose = [2.08, -0.65, 0.36, 0, 0, 0, 1]
obj12_dimensions = [0.27, 0.4, 0.12]
con_bottom_pose = [1.9, -0.5, 0.012, 0, 0, 0, 1]
con_bottom_dimensions = [0.704, 0.80, 0.02]
con_left_pose = [1.9, -0.9, 0.705, 0, 0, 0, 1]
con_left_dimensions = [0.744, 0.044, 1.386]
con_right_pose = [2.252, -0.5, 0.705, 0, 0, 0, 1]
con_right_dimensions = [0.044, 0.8, 1.386]
con_back_pose = [1.548, -0.5, 0.705, 0, 0, 0, 1]
con_back_dimensions = [0.044, 0.8, 1.386]
con2_bottom_pose = [0.3, -0.5, 0.012, 0, 0, 0, 1]
con2_bottom_dimensions = [0.704, 0.80, 0.02]
con2_left_pose = [0.3, -0.9, 0.705, 0, 0, 0, 1]
con2_left_dimensions = [0.744, 0.044, 1.386]
con2_right_pose = [0.652, -0.5, 0.705, 0, 0, 0, 1]
con2_right_dimensions = [0.044, 0.8, 1.386]
con2_back_pose = [-0.052, -0.5, 0.705, 0, 0, 0, 1]
con2_back_dimensions = [0.044, 0.8, 1.386]
con3_bottom_pose = [1.1, -0.5, 0.012, 0, 0, 0, 1]
con3_bottom_dimensions = [0.704, 0.80, 0.02]
con3_left_pose = [1.1, -0.9, 0.705, 0, 0, 0, 1]
con3_left_dimensions = [0.744, 0.044, 1.386]
con3_right_pose = [1.452, -0.5, 0.705, 0, 0, 0, 1]
con3_right_dimensions = [0.044, 0.8, 1.386]
con3_back_pose = [0.748, -0.5, 0.705, 0, 0, 0, 1]
con3_back_dimensions = [0.044, 0.8, 1.386]
self.add_box_object("wall1", wall1_dimensions, wall1_pose)
self.add_box_object("floor", floor_dimensions, floor_pose)
self.add_box_object("wall3", wall3_dimensions, wall3_pose)
self.add_box_object("wall2", wall2_dimensions, wall2_pose)
self.add_box_object("obj3", obj3_dimensions, obj3_pose)
self.add_box_object("obj2", obj2_dimensions, obj2_pose)
self.add_box_object("obj1", obj1_dimensions, obj1_pose)
self.add_box_object("obj4", obj4_dimensions, obj4_pose)
self.add_box_object("obj5", obj5_dimensions, obj5_pose)
self.add_box_object("obj10", obj10_dimensions, obj10_pose)
self.add_box_object("obj11", obj11_dimensions, obj11_pose)
self.add_box_object("obj12", obj12_dimensions, obj12_pose)
self.add_box_object("con_bottom", con_bottom_dimensions,
con_bottom_pose)
self.add_box_object("con_left", con_left_dimensions, con_left_pose)
self.add_box_object("con_right", con_right_dimensions, con_right_pose)
self.add_box_object("con_back", con_back_dimensions, con_back_pose)
self.add_box_object("con2_bottom", con2_bottom_dimensions,
con2_bottom_pose)
self.add_box_object("con2_left", con2_left_dimensions, con2_left_pose)
self.add_box_object("con2_right", con2_right_dimensions,
con2_right_pose)
self.add_box_object("con2_back", con2_back_dimensions, con2_back_pose)
self.add_box_object("con3_bottom", con3_bottom_dimensions,
con3_bottom_pose)
self.add_box_object("con3_left", con3_left_dimensions, con3_left_pose)
self.add_box_object("con3_right", con3_right_dimensions,
con3_right_pose)
self.add_box_object("con3_back", con3_back_dimensions, con3_back_pose)
print "========== Added test 4 scene!!"
def add_pap5(self):
floor_pose = [1.0, -1.0, -0.17, 0, 0, 0, 1]
floor_dimensions = [5.0, 4.0, 0.02]
wall1_pose = [1.0, 1.0, 0.92, 0, 0, 0, 1]
wall1_dimensions = [5.0, 0.02, 2.18]
wall2_pose = [3.5, -1.0, 0.92, 0, 0, 0, 1]
wall2_dimensions = [0.02, 4.0, 2.18]
wall3_pose = [-1.5, -1.0, 0.92, 0, 0, 0, 1]
wall3_dimensions = [0.02, 4.0, 2.18]
obj1_pose = [0.3, 0.6, -0.08, 0, 0, 0, 1]
obj1_dimensions = [0.27, 0.4, 0.12]
obj2_pose = [0.3, 0.21, -0.08, 0, 0, 0, 1]
obj2_dimensions = [0.27, 0.4, 0.12]
obj3_pose = [0.6, 0.6, -0.08, 0, 0, 0, 1]
obj3_dimensions = [0.27, 0.4, 0.12]
obj4_pose = [0.6, 0.21, -0.08, 0, 0, 0, 1]
obj4_dimensions = [0.27, 0.4, 0.12]
obj5_pose = [0.45, 0.40, 0.05, 0, 0, 0, 1]
obj5_dimensions = [0.27, 0.4, 0.12]
obj10_pose = [2.08, -0.65, 0.1, 0, 0, 0, 1]
obj10_dimensions = [0.27, 0.4, 0.12]
obj11_pose = [2.08, -0.65, 0.23, 0, 0, 0, 1]
obj11_dimensions = [0.27, 0.4, 0.12]
obj12_pose = [2.08, -0.65, 0.36, 0, 0, 0, 1]
obj12_dimensions = [0.27, 0.4, 0.12]
obs_pose = [2.1, -0.1, 0.35, 0, 0, 0, 1]
obs_dimensions = [0.1, 0.1, 0.9]
con_bottom_pose = [1.9, -0.5, 0.012, 0, 0, 0, 1]
con_bottom_dimensions = [0.704, 0.80, 0.02]
con_left_pose = [1.9, -0.9, 0.705, 0, 0, 0, 1]
con_left_dimensions = [0.744, 0.044, 1.386]
con_right_pose = [2.252, -0.5, 0.705, 0, 0, 0, 1]
con_right_dimensions = [0.044, 0.8, 1.386]
con_back_pose = [1.548, -0.5, 0.705, 0, 0, 0, 1]
con_back_dimensions = [0.044, 0.8, 1.386]
con2_bottom_pose = [0.3, -0.5, 0.012, 0, 0, 0, 1]
con2_bottom_dimensions = [0.704, 0.80, 0.02]
con2_left_pose = [0.3, -0.9, 0.705, 0, 0, 0, 1]
con2_left_dimensions = [0.744, 0.044, 1.386]
con2_right_pose = [0.652, -0.5, 0.705, 0, 0, 0, 1]
con2_right_dimensions = [0.044, 0.8, 1.386]
con2_back_pose = [-0.052, -0.5, 0.705, 0, 0, 0, 1]
con2_back_dimensions = [0.044, 0.8, 1.386]
con3_bottom_pose = [1.1, -0.5, 0.012, 0, 0, 0, 1]
con3_bottom_dimensions = [0.704, 0.80, 0.02]
con3_left_pose = [1.1, -0.9, 0.705, 0, 0, 0, 1]
con3_left_dimensions = [0.744, 0.044, 1.386]
con3_right_pose = [1.452, -0.5, 0.705, 0, 0, 0, 1]
con3_right_dimensions = [0.044, 0.8, 1.386]
con3_back_pose = [0.748, -0.5, 0.705, 0, 0, 0, 1]
con3_back_dimensions = [0.044, 0.8, 1.386]
self.add_box_object("wall1", wall1_dimensions, wall1_pose)
self.add_box_object("floor", floor_dimensions, floor_pose)
self.add_box_object("wall3", wall3_dimensions, wall3_pose)
self.add_box_object("wall2", wall2_dimensions, wall2_pose)
self.add_box_object("obj3", obj3_dimensions, obj3_pose)
self.add_box_object("obj2", obj2_dimensions, obj2_pose)
self.add_box_object("obj1", obj1_dimensions, obj1_pose)
self.add_box_object("obj4", obj4_dimensions, obj4_pose)
self.add_box_object("obj5", obj5_dimensions, obj5_pose)
self.add_box_object("obj10", obj10_dimensions, obj10_pose)
self.add_box_object("obj11", obj11_dimensions, obj11_pose)
self.add_box_object("obj12", obj12_dimensions, obj12_pose)
self.add_box_object("obs", obs_dimensions, obs_pose)
self.add_box_object("con_bottom", con_bottom_dimensions,
con_bottom_pose)
self.add_box_object("con_left", con_left_dimensions, con_left_pose)
self.add_box_object("con_right", con_right_dimensions, con_right_pose)
self.add_box_object("con_back", con_back_dimensions, con_back_pose)
self.add_box_object("con2_bottom", con2_bottom_dimensions,
con2_bottom_pose)
self.add_box_object("con2_left", con2_left_dimensions, con2_left_pose)
self.add_box_object("con2_right", con2_right_dimensions,
con2_right_pose)
self.add_box_object("con2_back", con2_back_dimensions, con2_back_pose)
self.add_box_object("con3_bottom", con3_bottom_dimensions,
con3_bottom_pose)
self.add_box_object("con3_left", con3_left_dimensions, con3_left_pose)
self.add_box_object("con3_right", con3_right_dimensions,
con3_right_pose)
self.add_box_object("con3_back", con3_back_dimensions, con3_back_pose)
print "========== Added test 5 scene!!"
def add_box_object(self, name, dimensions, pose):
p = geometry_msgs.msg.PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = pose[0]
p.pose.position.y = pose[1]
p.pose.position.z = pose[2]
p.pose.orientation.x = pose[3]
p.pose.orientation.y = pose[4]
p.pose.orientation.z = pose[5]
p.pose.orientation.w = pose[6]
self._scene.add_box(name, p,
(dimensions[0], dimensions[1], dimensions[2]))
if name == "obj5":
print True
self._scene.attach_box("pressure_box", name)
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
scene = PlanningSceneInterface()
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
self.colors = dict()
rospy.sleep(1)
arm = MoveGroupCommander('arm')
end_effector_link = arm.get_end_effector_link()
arm.set_goal_position_tolerance(0.005)
arm.set_goal_orientation_tolerance(0.025)
arm.allow_replanning(True)
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
arm.set_planning_time(5)
#scene planning
table_id = 'table'
#cylinder_id='cylinder'
#box1_id='box1'
box2_id = 'box2'
target_id = 'target_object'
#scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(table_id)
scene.remove_world_object(target_id)
rospy.sleep(2)
table_ground = 0.68
table_size = [0.5, 1, 0.01]
#box1_size=[0.1,0.05,0.03]
box2_size = [0.05, 0.05, 0.1]
r_tool_size = [0.03, 0.01, 0.06]
l_tool_size = [0.03, 0.01, 0.06]
target_size = [0.05, 0.05, 0.1]
table_pose = PoseStamped()
table_pose.header.frame_id = reference_frame
table_pose.pose.position.x = 0.75
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
'''
box1_pose=PoseStamped()
box1_pose.header.frame_id=reference_frame
box1_pose.pose.position.x=0.7
box1_pose.pose.position.y=-0.2
box1_pose.pose.position.z=table_ground+table_size[2]+box1_size[2]/2.0
box1_pose.pose.orientation.w=1.0
scene.add_box(box1_id,box1_pose,box1_size)
'''
box2_pose = PoseStamped()
box2_pose.header.frame_id = reference_frame
box2_pose.pose.position.x = 0.6
box2_pose.pose.position.y = -0.05
box2_pose.pose.position.z = table_ground + table_size[
2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.pose.position.x = 0.6
target_pose.pose.position.y = 0.05
target_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
target_pose.pose.orientation.x = 0
target_pose.pose.orientation.y = 0
target_pose.pose.orientation.z = 0
target_pose.pose.orientation.w = 1
scene.add_box(target_id, target_pose, target_size)
#left gripper
l_p = PoseStamped()
l_p.header.frame_id = end_effector_link
l_p.pose.position.x = 0.00
l_p.pose.position.y = 0.04
l_p.pose.position.z = 0.04
l_p.pose.orientation.w = 1
scene.attach_box(end_effector_link, 'l_tool', l_p, l_tool_size)
#right gripper
r_p = PoseStamped()
r_p.header.frame_id = end_effector_link
r_p.pose.position.x = 0.00
r_p.pose.position.y = -0.04
r_p.pose.position.z = 0.04
r_p.pose.orientation.w = 1
scene.attach_box(end_effector_link, 'r_tool', r_p, r_tool_size)
#grasp
g_p = PoseStamped()
g_p.header.frame_id = end_effector_link
g_p.pose.position.x = 0.00
g_p.pose.position.y = -0.00
g_p.pose.position.z = 0.025
g_p.pose.orientation.w = 0.707
g_p.pose.orientation.x = 0
g_p.pose.orientation.y = -0.707
g_p.pose.orientation.z = 0
#set color
self.setColor(table_id, 0.8, 0, 0, 1.0)
#self.setColor(box1_id,0.8,0.4,0,1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
self.setColor('r_tool', 0.8, 0, 0)
self.setColor('l_tool', 0.8, 0, 0)
self.setColor('target_object', 0, 1, 0)
self.sendColors()
#motion planning
arm.set_named_target("initial_arm1")
arm.go()
rospy.sleep(2)
grasp_pose = target_pose
grasp_pose.pose.position.x -= 0.15
#grasp_pose.pose.position.z=
grasp_pose.pose.orientation.x = 0
grasp_pose.pose.orientation.y = 0.707
grasp_pose.pose.orientation.z = 0
grasp_pose.pose.orientation.w = 0.707
arm.set_start_state_to_current_state()
arm.set_pose_target(grasp_pose, end_effector_link)
traj = arm.plan()
arm.execute(traj)
rospy.sleep(2)
print arm.get_current_joint_values()
#arm.shift_pose_target(4,1.57,end_effector_link)
#arm.go()
#rospy.sleep(2)
arm.shift_pose_target(0, 0.11, end_effector_link)
arm.go()
rospy.sleep(2)
print arm.get_current_joint_values()
saved_target_pose = arm.get_current_pose(end_effector_link)
#arm.set_named_target("initial_arm2")
#grasp
scene.attach_box(end_effector_link, target_id, g_p, target_size)
rospy.sleep(2)
#grasping is over , from now is placing
arm.shift_pose_target(2, 0.15, end_effector_link)
arm.go()
rospy.sleep(2)
print arm.get_current_joint_values()
arm.shift_pose_target(1, -0.2, end_effector_link)
arm.go()
rospy.sleep(2)
print arm.get_current_joint_values()
arm.shift_pose_target(2, -0.15, end_effector_link)
arm.go()
rospy.sleep(2)
print arm.get_current_joint_values()
scene.remove_attached_object(end_effector_link, target_id)
rospy.sleep(2)
#arm.set_pose_target(saved_target_pose,end_effector_link)
#arm.go()
#rospy.sleep(2)
arm.set_named_target("initial_arm1")
arm.go()
rospy.sleep(2)
#remove and shut down
scene.remove_attached_object(end_effector_link, 'l_tool')
scene.remove_attached_object(end_effector_link, 'r_tool')
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
class EdPick:
def __init__(self):
allowed_planning_time = 0.0
ARM_AND_LINEAR_GROUP_NAME = "arm_and_linear"
ARM_GROUP_NAME = "arm_torso"
GRIPPER_GROUP = "gripper"
REFERENCE_FRAME = "base_link"
PLANNER = "RRTConnectkConfigDefault"
#PLANNER = "RRTstarkConfigDefault"
MOVE_GROUP_SERVICE = "/move_group/plan_execution/set_parameters"
result = False
upright_constraints = None
robot = None
arm = None
arm_and_linear = None
database = None
scene = None
grasp_generator = None
place_generator = None
marker_publisher = None
self.robot = RobotCommander()
self.arm = self.robot.get_group("arm_torso")
self.gripper = self.robot.get_group(GRIPPER_GROUP)
self.scene = PlanningSceneInterface()
self.pickup_object = RecognizedObject("part")
self.table = EntityInfo()
def get_object(self):
rospy.wait_for_service('/ed/kinect/update')
self.update_client = rospy.ServiceProxy('/ed/kinect/update', Update)
self.request_update = UpdateRequest()
self.request_update.area_description = "on_top_of dinner-table"
self.result_update = self.update_client(self.request_update)
print "Found IDs: " % self.result_update.new_ids
rospy.wait_for_service('/ed/simple_query')
self.query_client = rospy.ServiceProxy('/ed/simple_query', SimpleQuery)
self.query_request = SimpleQueryRequest()
self.query_request.id = self.result_update.new_ids[0]
self.query_response = self.query_client(self.query_request)
rospy.wait_for_service('/ed/simple_query')
self.ed_simple = rospy.ServiceProxy('/ed/simple_query', SimpleQuery)
simpleRequest = SimpleQueryRequest()
simpleRequest.id = 'dinner-table'
self.table = self.ed_simple(simpleRequest).entities[0]
print self.table.pose
def pick_object(self):
# self.object = self.query_response.entities
self.pickup_object = RecognizedObject("part")
tmpPoseStamped = PoseStamped()
tmpPoseStamped.header.frame_id = '/map'
tmpPoseStamped.header.stamp = rospy.Time(0)
tmpPoseStamped.pose = self.query_response.entities[0].pose
self.pickup_obj.pose = tmpPoseStamped
self.value_x = []
self.value_y = []
self.value_z = []
for j in range(0, len(self.query_response.entities[0].convex_hull)):
self.value_x.append(
self.query_response.entities[0].convex_hull[j].x)
self.value_y.append(
self.query_response.entities[0].convex_hull[j].y)
self.value_z.append(
self.query_response.entities[0].convex_hull[j].z)
self.dim_x = max(self.value_x) - min(self.value_x)
self.dim_y = max(self.value_y) - min(self.value_y)
self.dim_z = max(self.value_z) - min(self.value_z)
self.pickup_object.dimensions = (self.dim_x, self.dim_y, self.dim_z)
self.add_to_planning_scene(self.pickup_object)
self.grasps = self.generate_grasps_for_object(self.pickup_object)
if not self.pickup(self.pickup_object, self.grasps) == -1:
print "Executed pick action"
self.attach_object_to_gripper(self.pickup_object)
self.place_location.header = tmpPoseStamped.header
self.place_location.pose.position.x = self.result.entityInfos[
i].pose.position.x
self.place_location.pose.position.y = self.result.entityInfos[
i].pose.position.y + 0.1
self.place_location.pose.position.z = self.result.entityInfos[
i].pose.position.z
self.place_location.pose.orientation.x = self.result.entityInfos[
i].pose.orientation.x
self.place_location.pose.orientation.y = self.result.entityInfos[
i].pose.orientation.y
self.place_location.pose.orientation.z = self.result.entityInfos[
i].pose.orientation.z
self.place_location.pose.orientation.w = self.result.entityInfos[
i].pose.orientation.w
myTable = MyTable(tmpPoseStamped.header, self.table.pose,
self.table.convex_hull)
self.place(self.recog_obj, myTable)
print "Executed place action"
def add_to_planning_scene(self, recognized_object):
pose_stamped = copy.deepcopy(recognized_object.pose)
self.scene.add_box(
recognized_object.name, pose_stamped,
(recognized_object.dimensions[0], recognized_object.dimensions[1],
recognized_object.dimensions[2]))
rospy.loginfo("Object {} added to planning scene".format(
recognized_object.name))
rospy.logdebug(
"Object {} added with pose \n{} \nand dimensions {}".format(
recognized_object.name, pose_stamped,
recognized_object.dimensions))
def attach_object_to_gripper(self, recognized_object):
rospy.loginfo("Attach object to gripper")
pose_stamped = copy.deepcopy(recognized_object.pose)
# self.remove_attached_object(recognized_object.name)
self.scene.remove_world_object(recognized_object.name)
self.scene.attach_box(
END_EFFECTOR_LINK_NAME, recognized_object.name, pose_stamped,
(recognized_object.dimensions[0], recognized_object.dimensions[1],
recognized_object.dimensions[2]),
GRIPPER_LINKS_TO_IGNORE_COLLISION)
def pickup(self, pickup_object, grasps):
supp_surface = 'table',
rospy.loginfo("Recognized object: " + str(pick_obj))
self.arm.set_support_surface_name(supp_surface)
result = self.arm.pick(pick_object.name, grasps)
self.print_moveit_result(result)
return result
def generate_grasps_for_object(self, recognized_object):
self.recognized_object = recognized_object
self.base_grasp_pose_stamped = copy.deepcopy(
self.recognized_object.pose)
self.horizontal_center_grasp_pose_stamped = self.base_grasp_pose_stamped
self.temp_grasp_pose_stamped = self.base_grasp_pose_stamped
self.grasp_quality = 1.0
if recognized_object.dimensions[2] < self.high_res_threshold:
rospy.loginfo(
"Z axis of object ({} m) is shorter than threshold {}. \
create denser grasp poses.".format(
recognized_object.dimensions[2], self.high_res_threshold))
self.distance_vertical = self.distance_vertical_default / 2.0
self.padding_from_top_and_bottom = self.padding_from_top_and_bottom_default / 2.0
else:
rospy.loginfo(
"Z axis of object ({} m) is longer than threshold {}. \
create normal distance grasp poses.".format(
recognized_object.dimensions[2], self.high_res_threshold))
self.distance_vertical = self.distance_vertical_default
self.padding_from_top_and_bottom = self.padding_from_top_and_bottom_default
self.number_of_poses_vertical = int(
(recognized_object.dimensions[2] -
float(self.padding_from_top_and_bottom)) /
float(self.distance_vertical) / 2.0)
rospy.loginfo("Number of vertical poses " +
str(self.number_of_poses_vertical))
# Generate the central grasp pose. Based on this pose the other poses are generated.
# check if this is a top grasp (x axis faces in negative z direction of base_link) or a side grasp
# pose with x downward, z forward
pose_stamped_compare = PoseStamped()
pose_stamped_compare.header.frame_id = "base_link"
pose_stamped_compare.pose.position.x = 0.0
pose_stamped_compare.pose.position.y = 0.0
pose_stamped_compare.pose.position.z = 0.0
pose_stamped_compare.pose.orientation.w = 0.7071
pose_stamped_compare.pose.orientation.x = 0.0
pose_stamped_compare.pose.orientation.y = 0.7071
pose_stamped_compare.pose.orientation.z = 0.0
angle = self.angle_between_poses(self.temp_grasp_pose_stamped,
pose_stamped_compare)
if angle < pi / 4:
# do a pinch grasp
rospy.loginfo("This grasp is a top grasp. Do a pinch grasp")
self.x_offset = -recognized_object.dimensions[
0] / 2 + self.palm_to_finger_tips_distance
self.recognized_object.top_grasp = True
else:
rospy.loginfo("This grasp is a side grasp. Do a power grasp")
self.x_offset = recognized_object.dimensions[
0] / 2 + self.additional_offset_in_grasp_direction
self.recognized_object.top_grasp = False
self.shift_pose_along_the_grippers_axes(self.temp_grasp_pose_stamped,
[-self.x_offset, 0, 0])
self.create_grasp(self.temp_grasp_pose_stamped)
# vertical
for vertical_counter in range(0, self.number_of_poses_vertical + 1):
if self.recognized_object.rotational_symmetric:
self.generate_horizontal_grasps(True)
self.generate_vertical_grasp_upper_part(vertical_counter)
self.generate_horizontal_grasps(True)
self.generate_vertical_grasp_lower_part(vertical_counter)
else:
self.generate_horizontal_grasps(False)
self.generate_vertical_grasp_upper_part(vertical_counter)
self.generate_horizontal_grasps(False)
self.generate_vertical_grasp_lower_part(vertical_counter)
rospy.loginfo("Generated vertical pose " + str(vertical_counter))
rospy.loginfo("Number of generated grasps: " + str(len(self.grasps)))
for grasp in self.grasps:
# rospy.logdebug("Publishing grasp z position:" + str(grasp.grasp_pose.pose.position))
# rospy.loginfo("Grasp quality: %s" % (grasp.grasp_quality))
return self.grasps
def generate_vertical_grasp_upper_part(self, vertical_counter):
self.generate_vertical_grasp("upper", vertical_counter)
def generate_vertical_grasp_lower_part(self, vertical_counter):
self.generate_vertical_grasp("lower", vertical_counter)
def generate_vertical_grasp(self, part, vertical_counter):
if part == "upper":
distance_vertical = self.distance_vertical
elif part == "lower":
distance_vertical = -self.distance_vertical
else:
rospy.logerror('Part %s unkown. Only use "upper" or "lower"' %
(part))
self.horizontal_center_grasp_pose_stamped = copy.deepcopy(
self.base_grasp_pose_stamped)
self.temp_grasp_pose_stamped = self.horizontal_center_grasp_pose_stamped
marvin_manipulation.transformations_common.shift_pose_along_the_grippers_axes(
self.temp_grasp_pose_stamped,
[0, 0, float(distance_vertical) * float(vertical_counter)])
self.grasp_quality = 1.0 / (float(vertical_counter) + 0.001
) #avoid devided by zero
self.create_grasp(self.temp_grasp_pose_stamped)
def generate_horizontal_grasps(self, rotational_symmetric):
if rotational_symmetric:
rospy.loginfo('Rotational symmetric')
for horizontal_counter in range(
-self.number_of_poses_horizontal,
self.number_of_poses_horizontal + 1):
self.temp_grasp_pose_stamped = copy.deepcopy(
self.horizontal_center_grasp_pose_stamped)
self.temp_grasp_pose_stamped = self.rotate_pose_around_object_center(
self.temp_grasp_pose_stamped,
self.angle_horizontal * float(horizontal_counter),
self.x_offset)
self.create_grasp(self.temp_grasp_pose_stamped)
else:
rospy.loginfo('non symmetric')
#create grasp facing in negative x direction
self.temp_grasp_pose_stamped = copy.deepcopy(
self.horizontal_center_grasp_pose_stamped)
self.temp_grasp_pose_stamped = self.rotate_pose_around_object_center(
self.temp_grasp_pose_stamped, pi, self.x_offset)
self.create_grasp(self.temp_grasp_pose_stamped)
def rotate_pose_around_object_center(self, pose_stamped, angle,
distance_to_object_center):
self.shift_pose_along_the_grippers_axes(
pose_stamped, (distance_to_object_center, 0, 0))
self.rotate_pose_around_axis_by_angle(pose_stamped, angle, (0, 0, 1))
self.shift_pose_along_the_grippers_axes(
pose_stamped, (-distance_to_object_center, 0, 0))
return pose_stamped
def create_grasp(self, pose_stamped):
grasp = Grasp()
grasp.grasp_pose = copy.deepcopy(pose_stamped)
# pre grasp gripper configuration
grasp.pre_grasp_posture.header.frame_id = self.gripper_frame
grasp.pre_grasp_posture.header.stamp = rospy.Time.now()
grasp.pre_grasp_posture.joint_names = self.finger_joint_names
grasp.pre_grasp_posture.points.append(
JointTrajectoryPoint(positions=[
self.gripper_open_angle_radian, self.gripper_open_angle_radian
],
effort=[self.max_effort, self.max_effort]))
# approach
grasp.pre_grasp_approach.direction.header.frame_id = self.gripper_frame
grasp.pre_grasp_approach.direction.vector.x = 1.0
grasp.pre_grasp_approach.min_distance = 0.04
grasp.pre_grasp_approach.desired_distance = 0.15
# grasp gripper configuration
grasp.grasp_posture.header.frame_id = self.gripper_frame
grasp.grasp_posture.header.stamp = rospy.Time.now()
grasp.grasp_posture.joint_names = self.finger_joint_names
grasp.grasp_posture.points.append(
JointTrajectoryPoint(positions=[
self.gripper_closed_angle_radian,
self.gripper_closed_angle_radian
],
effort=[self.max_effort, self.max_effort]))
# TODO: add time_from_start here and for gripper open
# retreat
grasp.post_grasp_retreat.direction.header.frame_id = "base_link"
grasp.post_grasp_retreat.direction.vector.z = 1.0
grasp.post_grasp_retreat.min_distance = 0.03
grasp.post_grasp_retreat.desired_distance = 0.05
# objects allowed to touch while approaching
grasp.allowed_touch_objects = [self.recognized_object.name]
grasp.grasp_quality = self.grasp_quality
self.grasps.append(grasp)
def angle_between_poses(pose_stamped, pose_stamped_compare=None):
if not pose_stamped_compare:
#pose with x forward, z upward
pose_stamped_compare = PoseStamped()
pose_stamped_compare.header.frame_id = "base_link"
pose_stamped_compare.pose.position.x = 0.0
pose_stamped_compare.pose.position.y = 0.0
pose_stamped_compare.pose.position.z = 0.0
pose_stamped_compare.pose.orientation.w = 1.0
pose_stamped_compare.pose.orientation.x = 0.0
pose_stamped_compare.pose.orientation.y = 0.0
pose_stamped_compare.pose.orientation.z = 0.0
quaternion = [
pose_stamped.pose.orientation.x, pose_stamped.pose.orientation.y,
pose_stamped.pose.orientation.z, pose_stamped.pose.orientation.w
]
compare_quaternion = [
pose_stamped_compare.pose.orientation.x,
pose_stamped_compare.pose.orientation.y,
pose_stamped_compare.pose.orientation.z,
pose_stamped_compare.pose.orientation.w
]
inner_product = numpy.inner(quaternion, compare_quaternion)
angle_between_poses = math.degrees(math.acos(2 * inner_product**2 - 1))
print("Angle between poses: %s" % angle_between_poses)
return angle_between_poses
def shift_pose_along_the_grippers_axes(pose_stamped, shift_per_axis_array):
'''
Shifts given pose_stamped by the amounts given in shift_per_axis_array
:param PoseStamped pose_stamped:
:param () shift_per_axis_array: tupel of translations e.g. (0.5, 0, 0) to shift the pose 0.5m in x direction
'''
frame = PyKDL.Frame(
PyKDL.Rotation.Quaternion(pose_stamped.pose.orientation.x,
pose_stamped.pose.orientation.y,
pose_stamped.pose.orientation.z,
pose_stamped.pose.orientation.w),
PyKDL.Vector(pose_stamped.pose.position.x,
pose_stamped.pose.position.y,
pose_stamped.pose.position.z))
point = PyKDL.Vector(shift_per_axis_array[0], shift_per_axis_array[1],
shift_per_axis_array[2])
point = frame * point
pose_stamped.pose.position.x = point.x()
pose_stamped.pose.position.y = point.y()
pose_stamped.pose.position.z = point.z()
def place(self, recognized_object, pose_stamped):
result = self.arm.place(recognized_object.name, pose_stamped)
rospy.loginfo("Place result: " + str(result))
if result == True:
return result
return result
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
# Construct the initial scene object
scene = PlanningSceneInterface()
rospy.sleep(1)
# Connect to the right_arm move group
right_arm = moveit_commander.MoveGroupCommander('arm')
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Display the name of the end_effector link
rospy.loginfo("The end effector link is: " + str(end_effector_link))
# Set a small tolerance on joint angles
right_arm.set_goal_joint_tolerance(0.001)
right_arm.set_goal_position_tolerance(0.0001)
for i in range(5):
scene.remove_attached_object('block' + str(i+1))
scene.remove_world_object('block' + str(i+1))
rospy.sleep(0.5)
# Start the arm target in "right" pose stored in the SRDF file
right_arm.set_named_target('right')
# Plan a trajectory to the goal configuration
traj = right_arm.plan()
# Execute the planned trajectory
right_arm.execute(traj)
# Pause for a moment
rospy.sleep(1)
target_pose = right_arm.get_current_pose()
target_pose.pose.position.x -= 0.2
target_pose.pose.position.y += 0.5
target_pose.pose.position.z -= 0.5
target_pose.pose.orientation.w = 1
count = 0
while count < 2:
# Set the size of block
block_size = [0.01, 0.05, 0.05]
count += 1
scene.add_box('block'+str(count), target_pose, block_size)
self.move_arm(right_arm, 1)
rospy.sleep(2)
# Create a pose for the block relative to the end-effector
p = PoseStamped()
p.header.frame_id = end_effector_link
# Place the end of the object within the grasp of the gripper
p.pose.position.x = -0.015
p.pose.position.y = 0.0
p.pose.position.z = 0.0
# Align the object with the gripper (straight out)
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1
# Attach the tool to the end-effector
scene.attach_box(end_effector_link, 'block'+str(count), p, block_size)
rospy.sleep(1)
self.move_arm(right_arm, 2)
rospy.sleep(1)
right_arm.shift_pose_target(1, 0.025*(count-1), end_effector_link)
right_arm.go()
rospy.sleep(1)
scene.remove_attached_object(end_effector_link, 'block'+str(count))
rospy.sleep(2)
#right_arm.shift_pose_target(0, -0.05, end_effector_link)
#right_arm.go()
#self.move_arm(right_arm, 3)
#rospy.sleep(2)
# Return the arm to the named "resting" pose stored in the SRDF file
right_arm.set_named_target('right')
right_arm.go()
rospy.sleep(1)
# Cleanly shut down MoveIt
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
scene=PlanningSceneInterface()
self.scene_pub=rospy.Publisher('planning_scene',PlanningScene)
self.colors=dict()
rospy.sleep(1)
self.arm=MoveGroupCommander('arm')
cartesian=rospy.get_param('~cartesian',True)
self.end_effector_link=self.arm.get_end_effector_link()
self.arm.set_goal_position_tolerance(0.005)
self.arm.set_goal_orientation_tolerance(0.025)
self.arm.allow_replanning(True)
self.reference_frame='base_link'
self.arm.set_pose_reference_frame(self.reference_frame)
self.arm.set_planning_time(5)
#scene planning
table_id='table'
#cylinder_id='cylinder'
#box1_id='box1'
box2_id='box2'
target_id='target_object'
#scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(table_id)
scene.remove_world_object(target_id)
rospy.sleep(2)
table_ground=0.20
table_size=[1,0.5,0.01]
#box1_size=[0.1,0.05,0.03]
#box2_size=[0.05,0.05,0.1]
r_tool_size=[0.03,0.01,0.06]
l_tool_size=[0.03,0.01,0.06]
target_size=[0.03,0.03,0.1]
table_pose=PoseStamped()
table_pose.header.frame_id=self.reference_frame
table_pose.pose.position.x=0
table_pose.pose.position.y=0.75
table_pose.pose.position.z=table_ground+table_size[2]/2.0
table_pose.pose.orientation.w=1.0
scene.add_box(table_id,table_pose,table_size)
'''
box1_pose=PoseStamped()
box1_pose.header.frame_id=reference_frame
box1_pose.pose.position.x=0.7
box1_pose.pose.position.y=-0.2
box1_pose.pose.position.z=table_ground+table_size[2]+box1_size[2]/2.0
box1_pose.pose.orientation.w=1.0
scene.add_box(box1_id,box1_pose,box1_size)
box2_pose=PoseStamped()
box2_pose.header.frame_id=self.reference_frame
box2_pose.pose.position.x=0.6
box2_pose.pose.position.y=-0.05
box2_pose.pose.position.z=table_ground+table_size[2]+box2_size[2]/2.0
box2_pose.pose.orientation.w=1.0
scene.add_box(box2_id,box2_pose,box2_size)
'''
#left gripper
l_p=PoseStamped()
l_p.header.frame_id=self.end_effector_link
l_p.pose.position.x=0.00
l_p.pose.position.y=0.04
l_p.pose.position.z=0.04
l_p.pose.orientation.w=1
scene.attach_box(self.end_effector_link,'l_tool',l_p,l_tool_size)
#right gripper
r_p=PoseStamped()
r_p.header.frame_id=self.end_effector_link
r_p.pose.position.x=0.00
r_p.pose.position.y=-0.04
r_p.pose.position.z=0.04
r_p.pose.orientation.w=1
scene.attach_box(self.end_effector_link,'r_tool',r_p,r_tool_size)
#target
target_pose=PoseStamped()
target_pose.header.frame_id=self.reference_frame
target_pose.pose.position.x=-0.4
target_pose.pose.position.y=0.65
target_pose.pose.position.z=table_ground+table_size[2]+target_size[2]/2.0
target_pose.pose.orientation.x=0
target_pose.pose.orientation.y=0
target_pose.pose.orientation.z=0
target_pose.pose.orientation.w=1
scene.add_box(target_id,target_pose,target_size)
#grasp
#grasp_pose(self,x,y,z,r,theta)
#set color
self.setColor(table_id,0.8,0,0,1.0)
#self.setColor(box1_id,0.8,0.4,0,1.0)
self.setColor(box2_id,0.8,0.4,0,1.0)
self.setColor('r_tool',0.8,0,0)
self.setColor('l_tool',0.8,0,0)
self.setColor('target_object',0,1,0)
self.sendColors()
#motion planning
j_ori_state=[-1.899937629699707, -0.5684762597084045, 0.46537330746650696, 2.3229329586029053, -0.057941947132349014, -1.2867668867111206, 0.2628822326660156]
self.arm.set_joint_value_target(j_ori_state)
self.arm.go()
rospy.sleep(3)
r=0.06
maxtries=300
for theta in range(0,157,16):
theta/=100.0
start_pose=self.arm.get_current_pose(self.end_effector_link)
rospy.sleep(2)
grasp_pose=self.grasp_pose(target_pose.pose.position.x,target_pose.pose.position.y,target_pose.pose.position.z,r,theta)
if cartesian:
(plan,e_j_state,s)=self.cts(start_pose.pose,grasp_pose.pose,maxtries)
if s==0: continue
#print(plan.joint_trajectory.points)
#self.arm.set_joint_value_target(e_j_state)
#self.arm.go()
self.arm.execute(plan)
rospy.sleep(5)
#self.arm.set_named_target("initial_arm")
#self.arm.go()
#rospy.sleep(2)
#remove and shut down
scene.remove_attached_object(self.end_effector_link,'l_tool')
scene.remove_attached_object(self.end_effector_link,'r_tool')
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# Initialize API for movo_group
moveit_commander.roscpp_initialize(sys.argv)
# Initialize Node of ROS
rospy.init_node('zhiyang_attached_camera_demo')
# Define Scene Object
scene = PlanningSceneInterface()
rospy.sleep(1)
# Initialize arm group from moveit group to control manipulator
rarm = MoveGroupCommander('right_arm')
larm = MoveGroupCommander('left_arm')
# Get end effector link name
r_end_effector_link = rarm.get_end_effector_link()
l_end_effector_link = larm.get_end_effector_link()
# Set tolerance of position(m) and orentation(rad)
rarm.set_goal_position_tolerance(0.01)
rarm.set_goal_orientation_tolerance(0.05)
larm.set_goal_position_tolerance(0.01)
larm.set_goal_orientation_tolerance(0.05)
# Allow replanning
rarm.allow_replanning(True)
rarm.set_planning_time(10)
larm.allow_replanning(True)
larm.set_planning_time(10)
# Let arm go back to home position
# arm.set_named_target('home')
# arm.go()
# Remove other attached object in before Scene run
# scene.remove_attached_object(end_effector_link, 'tool')
# scene.remove_world_object('table')
# scene.remove_world_object('target')
# Set height of table
table_ground = 0.6
# Set 3d size of table and tool
table_size = [0.3, 0.7, 0.01]
tool_size = [0.15, 0.15, 0.06]
# Set orentation and position of tool
p1 = PoseStamped()
p1.header.frame_id = r_end_effector_link
# p.pose.position.x = tool_size[0] / 2.0 - 0.025
# p.pose.position.y = -0.015
# p.pose.position.z = 0.0
p1.pose.position.x = -0.07
p1.pose.position.y = 0.0
p1.pose.position.z = 0.08
p1.pose.orientation.x = 0
p1.pose.orientation.y = 0
p1.pose.orientation.z = 0
p1.pose.orientation.w = 1
p2 = PoseStamped()
p2.header.frame_id = l_end_effector_link
p2.pose.position.x = -0.07
p2.pose.position.y = 0.0
p2.pose.position.z = 0.08
p2.pose.orientation.x = 0
p2.pose.orientation.y = 0
p2.pose.orientation.z = 0
p2.pose.orientation.w = 1
# Make tool attact to end effector
scene.attach_box(r_end_effector_link, 'Camera1', p1, tool_size)
scene.attach_box(l_end_effector_link, 'Camera2', p2, tool_size)
# Add table to Scene
table_pose = PoseStamped()
table_pose.header.frame_id = 'base_link'
table_pose.pose.position.x = 0.75
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box('table', table_pose, table_size)
rospy.sleep(2)
# Update current state (position and orentation)
rarm.set_start_state_to_current_state()
larm.set_start_state_to_current_state()
# Set target (rad)
joint_positions = [0.827228546495185, 0.29496592875743577, 0.29496592875743577, 1.1185644936946095, -0.7987583317769674, -0.18950024740190782, 0.11752152218233858]
rarm.set_joint_value_target(joint_positions)
larm.set_joint_value_target(joint_positions)
# Let arm go
rarm.go()
larm.go()
rospy.sleep(1)
# Let arm go back to initial state
# arm.set_named_target('home')
# arm.go()
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Construct the initial scene object
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=10)
# Create a dictionary to hold object colors
self.colors = dict()
# Pause for the scene to get ready
rospy.sleep(1)
# Initialize the move group for the right arm
left_arm = MoveGroupCommander('left_arm')
left_gripper = MoveGroupCommander('left_gripper')
# Get the name of the end-effector link
left_eef = left_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
left_arm.set_goal_position_tolerance(0.01)
left_arm.set_goal_orientation_tolerance(0.05)
# Allow replanning to increase the odds of a solution
left_arm.allow_replanning(True)
left_reference_frame = left_arm.get_planning_frame()
# Allow 5 seconds per planning attempt
left_arm.set_planning_time(5)
object1_id = 'object1'
obstacle1_id = 'obstacle1'
# Remove leftover objects from a previous run
scene.remove_world_object(object1_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
left_arm.set_named_target('left_arm_zero')
left_arm.go()
rospy.sleep(1)
left_gripper.set_joint_value_target(GRIPPER_OPEN)
left_gripper.go()
rospy.sleep(1)
object1_size = [0.088, 0.04, 0.02]
# Add a table top and two boxes to the scene
obstacle1_size = [0.3, 0.05, 0.45]
# Add a table top and two boxes to the scene
obstacle1_pose = PoseStamped()
obstacle1_pose.header.frame_id = left_reference_frame
obstacle1_pose.pose.position.x = 0.96
obstacle1_pose.pose.position.y = 0.5
obstacle1_pose.pose.position.z = 0.04
obstacle1_pose.pose.orientation.w = 1.0
scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size)
self.setColor(obstacle1_id, 0.8, 0.4, 0, 1.0)
object1_pose = PoseStamped()
object1_pose.header.frame_id = left_reference_frame
object1_pose.pose.position.x = 0.80
object1_pose.pose.position.y = 0.3
object1_pose.pose.position.z = 0
object1_pose.pose.orientation.w = 1.0
scene.add_box(object1_id, object1_pose, object1_size)
self.setColor(object1_id, 0.8, 0, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the start state to the current state
left_arm.set_start_state_to_current_state()
# Set the target pose in between the boxes and above the table
target_pose = PoseStamped()
target_pose.header.frame_id = left_reference_frame
target_pose.pose.position.x = 0.58
target_pose.pose.position.y = 0.1878
target_pose.pose.position.z = .1116
target_pose.pose.orientation.x = 0.1325
target_pose.pose.orientation.y = 0.9908
target_pose.pose.orientation.z = 0.0095
target_pose.pose.orientation.w = 0.0254
# Set the target pose for the arm
left_arm.set_pose_target(target_pose, left_eef)
# Move the arm to the target pose (if possible)
left_arm.go()
# Pause for a moment...
rospy.sleep(2)
left_gripper.set_joint_value_target(GRIPPER_CLOSE)
left_gripper.go()
rospy.sleep(1)
scene.attach_box(left_eef, object1_id, object1_pose, object1_size)
# Cartesian Paths
waypoints1 = []
start_pose = left_arm.get_current_pose(left_eef).pose
wpose = deepcopy(start_pose)
waypoints1.append(deepcopy(wpose))
wpose.position.x -= 0.05
wpose.position.y += 0.105
wpose.position.z += 0.07
waypoints1.append(deepcopy(wpose))
wpose.position.x -= 0.05
wpose.position.y += 0.105
wpose.position.z -= 0.07
waypoints1.append(deepcopy(wpose))
(cartesian_plan, fraction) = left_arm.compute_cartesian_path(
waypoints1, # waypoint poses
0.01, # eef_step 1cm
0.0, # jump_threshold
True) # avoid_collisions
left_arm.execute(cartesian_plan)
rospy.sleep(2)
left_gripper.set_joint_value_target(GRIPPER_OPEN)
left_gripper.go()
rospy.sleep(1)
scene.remove_attached_object(left_eef, object1_id)
# Exit MoveIt cleanly
waypoints2 = []
start_pose = left_arm.get_current_pose(left_eef).pose
wpose = deepcopy(start_pose)
waypoints2.append(deepcopy(wpose))
wpose.position.z += 0.07
waypoints2.append(deepcopy(wpose))
wpose.position.x += 0.1
wpose.position.y -= 0.21
waypoints2.append(deepcopy(wpose))
wpose.position.z -= 0.07
waypoints2.append(deepcopy(wpose))
(cartesian_plan, fraction) = left_arm.compute_cartesian_path(
waypoints2, # waypoint poses
0.01, # eef_step 1cm
0.0, # jump_threshold
True) # avoid_collisions
left_arm.execute(cartesian_plan)
rospy.sleep(2)
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def __init__(self):
rospy.init_node('moveit_demo')
#self.sub = rospy.Subscriber('/aruco_single/pose', Pose, self.update)
#self.sub = rospy.Subscriber('/aruco_single/pose', PoseArray, self.update, queue_size=1)
#msg = rospy.wait_for_message('/aruco_single/pose',PoseStamped)
#grasp msg publisher
m_pub_joint_g_1 = rospy.Publisher("/ik_solution_g_1",
JointTrajectoryPoint,
queue_size=1,
latch=True)
m_pub_joint_g_2 = rospy.Publisher("/ik_solution_g_2",
JointTrajectoryPoint,
queue_size=1,
latch=True)
m_pub_joint_g_3 = rospy.Publisher("/ik_solution_g_3",
JointTrajectoryPoint,
queue_size=1,
latch=True)
m_pub_joint_g_4 = rospy.Publisher("/ik_solution_g_4",
JointTrajectoryPoint,
queue_size=1,
latch=True)
m_pub_joint_g_5 = rospy.Publisher("/ik_solution_g_5",
JointTrajectoryPoint,
queue_size=1,
latch=True)
#place msg publisher
m_pub_joint_p_1 = rospy.Publisher("/ik_solution_p_1",
JointTrajectoryPoint,
queue_size=1,
latch=True)
m_pub_joint_p_2 = rospy.Publisher("/ik_solution_p_2",
JointTrajectoryPoint,
queue_size=1,
latch=True)
m_pub_joint_p_3 = rospy.Publisher("/ik_solution_p_3",
JointTrajectoryPoint,
queue_size=1,
latch=True)
m_pub_joint_p_4 = rospy.Publisher("/ik_solution_p_4",
JointTrajectoryPoint,
queue_size=1,
latch=True)
m_pub_joint_p_5 = rospy.Publisher("/ik_solution_p_5",
JointTrajectoryPoint,
queue_size=1,
latch=True)
m_pub_joint_p_6 = rospy.Publisher("/ik_solution_p_6",
JointTrajectoryPoint,
queue_size=1,
latch=True)
moveit_commander.roscpp_initialize(sys.argv)
cartesian = rospy.get_param('~cartesian', True)
scene = PlanningSceneInterface()
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
self.colors = dict()
'''
"""set up ros publishers"""
self.m_pub_m2j = rospy.Publisher("/two_finger/motoman_control/move_to_joint", JointAnglesDuration, queue_size=1, latch=True);
self.m_pub_m2p = rospy.Publisher("/two_finger/motoman_control/move_to_pos", PoseDuration, queue_size=1, latch=True)
self.m_pub_m2p_star = rospy.Publisher("/two_finger/motoman_control/move_to_pos_straight", PoseDuration, queue_size=1, latch=True)
""" set up ros service """
print("wait gripper service")
rospy.wait_for_service("/two_finger/gripper/gotoPosition")
rospy.wait_for_service("/two_finger/gripper/gotoPositionUntilTouch")
self.m_srv_grpCtrl = rospy.ServiceProxy("/two_finger/gripper/gotoPosition", SetPosition)
self.m_srv_grpCtrlUntilTouch = rospy.ServiceProxy("/two_finger/gripper/gotoPositionUntilTouch", SetPosition)
self.m_srv_grpSpeed = rospy.ServiceProxy("/two_finger/gripper/setSpeed", SetParameter)
self.m_srv_grpTorque = rospy.ServiceProxy("/two_finger/gripper/setTorque", SetParameter)
self.m_srv_grpTorque(20)
self.m_srv_grpSpeed(128)
rospy.wait_for_service("/two_finger/motoman_control/trajectory_status")
self.m_srv_trajectoryStatus = rospy.ServiceProxy("/two_finger/motoman_control/trajectory_status", Trigger)
'''
rospy.sleep(1)
arm = MoveGroupCommander('arm')
arm.allow_replanning(True)
end_effector_link = arm.get_end_effector_link()
arm.set_goal_position_tolerance(0.03)
arm.set_goal_orientation_tolerance(0.025)
arm.allow_replanning(True)
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
arm.set_planning_time(5)
#scene planning
table_id = 'table'
#cylinder_id='cylinder'
box_id = 'box'
#box2_id='box2'
target_id = 'target_object'
scene.remove_world_object(box_id)
#scene.remove_world_object(box2_id)
scene.remove_world_object(table_id)
#scene.remove_world_object(target_id)
scene.remove_world_object(target_id)
rospy.sleep(2)
#table_ground=0.62
table_size = [0.9, 0.6, 0.018]
box_size = [0.15, 0.15, 0.015]
#box2_size=[0.05,0.05,0.1]
r_tool_size = [0.03, 0.02, 0.18]
l_tool_size = [0.03, 0.02, 0.18]
target_size = [0.059, 0.059, 0.12]
table_pose = PoseStamped()
table_pose.header.frame_id = reference_frame
table_pose.pose.position.x = -0.05
table_pose.pose.position.y = 0.61
table_pose.pose.position.z = 0.194
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
'''
box1_pose=PoseStamped()
box1_pose.header.frame_id=reference_frame
box1_pose.pose.position.x=0.7
box1_pose.pose.position.y=-0.2
box1_pose.pose.position.z=table_ground+table_size[2]+box1_size[2]/2.0
box1_pose.pose.orientation.w=1.0
scene.add_box(box1_id,box1_pose,box1_size)
box2_pose=PoseStamped()
box2_pose.header.frame_id=reference_frame
box2_pose.pose.position.x=0.6
box2_pose.pose.position.y=-0.05
box2_pose.pose.position.z=table_ground+table_size[2]+box2_size[2]/2.0
box2_pose.pose.orientation.w=1.0
scene.add_box(box2_id,box2_pose,box2_size)
'''
#left gripper
l_p = PoseStamped()
l_p.header.frame_id = end_effector_link
l_p.pose.position.x = 0.00
l_p.pose.position.y = 0.057
l_p.pose.position.z = 0.09
l_p.pose.orientation.w = 1
scene.attach_box(end_effector_link, 'l_tool', l_p, l_tool_size)
#right gripper
r_p = PoseStamped()
r_p.header.frame_id = end_effector_link
r_p.pose.position.x = 0.00
r_p.pose.position.y = -0.057
r_p.pose.position.z = 0.09
r_p.pose.orientation.w = 1
scene.attach_box(end_effector_link, 'r_tool', r_p, r_tool_size)
#box
#place
box_pose = PoseStamped()
box_pose.header.frame_id = reference_frame
box_pose.pose.position.x = -0.30
box_pose.pose.position.y = 0.69
box_pose.pose.position.z = 0.21
box_pose.pose.orientation.w = 1
scene.add_box(box_id, box_pose, box_size)
#place
p_pose = box_pose
p_pose.pose.position.x += 0.01
p_pose.pose.position.y += 0.01
p_pose.pose.position.z += 0.11
p_pose.pose.orientation.w = -0.5
p_pose.pose.orientation.x = -0.5
p_pose.pose.orientation.y = -0.5
p_pose.pose.orientation.z = 0.5
#scene.add_box(box1_id,p_pose,box2_size)
#grasp
g_p = PoseStamped()
g_p.header.frame_id = end_effector_link
g_p.pose.position.x = 0
g_p.pose.position.y = 0
g_p.pose.position.z = 0.15
g_p.pose.orientation.w = 0.707
g_p.pose.orientation.x = -0
g_p.pose.orientation.y = -0.707
g_p.pose.orientation.z = 0.0
#set color
self.setColor(table_id, 0.8, 0, 0, 1.0)
#self.setColor(box1_id,0.8,0.4,0,1.0)
#self.setColor(box2_id,0.8,0.4,0,1.0)
self.setColor('r_tool', 0.8, 0, 0)
self.setColor('l_tool', 0.8, 0, 0)
self.sendColors()
#motion planning
dev = InputDevice('/dev/input/event4')
print "xxx"
while not rospy.is_shutdown():
duration = rospy.Duration(4)
duration_s = rospy.Duration(2.5)
select([dev], [], [])
for event in dev.read():
if (event.value == 0) and (event.code == 16):
print " Path planning"
scene.remove_world_object(target_id)
rospy.loginfo("Plz move the target")
msg = rospy.wait_for_message('/aruco_single/pose',
PoseStamped)
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.pose.position.x = -(msg.pose.position.y) - 0.28
target_pose.pose.position.y = -msg.pose.position.x + 0.81 - 0.068
target_pose.pose.position.z = 1.36 - msg.pose.position.z + 0.06
target_pose.pose.orientation.x = 0
target_pose.pose.orientation.y = 0
target_pose.pose.orientation.z = -0
target_pose.pose.orientation.w = 1
scene.add_box(target_id, target_pose, target_size)
self.setColor('target_object', 0, 1, 0)
#pre-grasp pose
pre_grasp_pose = PoseStamped()
pre_grasp_pose.header.frame_id = reference_frame
pre_grasp_pose.pose.position = target_pose.pose.position
pre_grasp_pose.pose.position.y -= 0.1
pre_grasp_pose.pose.position.z += 0.015
pre_grasp_pose.pose.orientation.x = -0.5
pre_grasp_pose.pose.orientation.y = -0.5
pre_grasp_pose.pose.orientation.z = -0.5
pre_grasp_pose.pose.orientation.w = 0.5
#grasp pose
grasp_pose = PoseStamped()
grasp_pose.header.frame_id = reference_frame
grasp_pose.pose.position = target_pose.pose.position
grasp_pose.pose.position.y -= 0.05
grasp_pose.pose.position.z += 0.01
'''
#orientation from left
grasp_pose.pose.orientation.x=-0.000149
grasp_pose.pose.orientation.y=-0.707
grasp_pose.pose.orientation.z=0
grasp_pose.pose.orientation.w=0.707
'''
#orientation from forward
grasp_pose.pose.orientation.x = -0.5
grasp_pose.pose.orientation.y = -0.5
grasp_pose.pose.orientation.z = -0.5
grasp_pose.pose.orientation.w = 0.5
arm.set_named_target("initial_arm")
arm.go()
rospy.sleep(2)
start_pose = arm.get_current_pose(end_effector_link).pose
#initial waypoints list
waypoints_1 = []
waypoints_2 = []
waypoints_3 = []
waypoints_4 = []
if cartesian:
waypoints_1.append(start_pose)
waypoints_1.append(deepcopy(pre_grasp_pose.pose))
fraction = 0.0
maxtries = 300
attempts = 0
#arm.set_start_state__1to_current_state()
#plan the cartesian path connecting waypoints
while fraction < 1.0 and attempts < maxtries:
(plan_1, fraction) = arm.compute_cartesian_path(
waypoints_1, 0.01, 0.0, True)
attempts += 1
if (attempts % 300 == 0 and fraction != 1.0):
rospy.loginfo(
"path planning failed with only " +
str(fraction * 100) + "% success after " +
str(maxtries) + " attempts")
continue
if fraction == 1.0:
rospy.loginfo(
"path compute successfully. Move the arm.")
#place = JointAnglesDuration(JointAngles(plan.joint_trajectory.points[len(plan.joint_trajectory)-1].positions), duration)
#self.m_pub_m2j.publish(place)
print
arm.execute(plan_1)
m_pub_joint_g_1.publish(
plan_1.joint_trajectory.points[-1])
m_pub_joint_g_2.publish(
plan_1.joint_trajectory.points[-2])
m_pub_joint_g_3.publish(
plan_1.joint_trajectory.points[-3])
m_pub_joint_g_4.publish(
plan_1.joint_trajectory.points[-5])
m_pub_joint_g_5.publish(
plan_1.joint_trajectory.points[-8])
rospy.loginfo("path execution complete. ")
scene.attach_box(end_effector_link, target_id,
g_p, target_size)
rospy.sleep(5)
g_pose = arm.get_current_pose(end_effector_link)
if cartesian:
waypoints_2.append(g_pose.pose)
waypoints_2.append(deepcopy(p_pose.pose))
fraction = 0.0
maxtries = 300
attempts = 0
#arm.set_start_state_to_current_state()
#plan the cartesian path connecting waypoints
while fraction < 1.0 and attempts < maxtries:
(plan_2, fraction) = arm.compute_cartesian_path(
waypoints_2, 0.01, 0.0, True)
attempts += 1
if (attempts % 300 == 0 and fraction != 1.0):
rospy.loginfo(
"path planning failed with only " +
str(fraction * 100) + "% success after " +
str(maxtries) + " attempts")
continue
if fraction == 1.0:
rospy.loginfo(
"path compute successfully. Move the arm.")
#place = JointAnglesDuration(JointAngles(plan.joint_trajectory.points[len(plan.joint_trajectory)-1].positions), duration)
#self.m_pub_m2j.publish(place)
arm.execute(plan_2)
m_pub_joint_p_1.publish(
plan_2.joint_trajectory.points[5])
m_pub_joint_p_1.publish(
plan_2.joint_trajectory.points[len(
plan_2.joint_trajectory.points) / 5])
m_pub_joint_p_1.publish(
plan_2.joint_trajectory.points[len(
plan_2.joint_trajectory.points) / 2])
m_pub_joint_p_1.publish(
plan_2.joint_trajectory.points[
-len(plan_2.joint_trajectory.points) /
5])
m_pub_joint_p_1.publish(
plan_2.joint_trajectory.points[5])
m_pub_joint_p_1.publish(
plan_2.joint_trajectory.points[1])
rospy.loginfo("path execution complete. ")
rospy.sleep(5)
#m_pub_joint_g.publish(arm.get_current_joint_value())
'''
arm.set_start_state_to_current_state()
arm.set_pose_target(grasp_pose,end_effector_link)
traj=arm.plan()
arm.execute(traj)
rospy.sleep(2)
print arm.get_current_joint_values()
#arm.shift_pose_target(4,1.57,end_effector_link)
#arm.go()
#rospy.sleep(2)
arm.shift_pose_target(0,0.11,end_effector_link)
arm.go()
rospy.sleep(2)
print arm.get_current_joint_values()
saved_target_pose=arm.get_current_pose(end_effector_link)
#arm.set_named_target("initial_arm2")
#grasp
scene.attach_box(end_effector_link,target_id,g_p,target_size)
rospy.sleep(2)
#grasping is over , from now is placing
arm.shift_pose_target(2,0.15,end_effector_link)
arm.go()
rospy.sleep(2)
print arm.get_current_joint_values()
arm.shift_pose_target(1,-0.2,end_effector_link)
arm.go()
rospy.sleep(2)
print arm.get_current_joint_values()
arm.shift_pose_target(2,-0.15,end_effector_link)
arm.go()
rospy.sleep(2)
print arm.get_current_joint_values()
scene.remove_attached_object(end_effector_link,target_id)
rospy.sleep(2)
#arm.set_pose_target(saved_target_pose,end_effector_link)
#arm.go()
#rospy.sleep(2)
arm.set_named_target("initial_arm1")
arm.go()
rospy.sleep(2)
'''
#remove and shut down
scene.remove_world_object(target_id)
scene.remove_world_object(table_id)
scene.remove_attached_object(end_effector_link, 'l_tool')
scene.remove_attached_object(end_effector_link, 'r_tool')
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_object', anonymous=True)
# 初始化场景对象，用来监听外部环境的变化
scene = PlanningSceneInterface()
rospy.sleep(1)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('dianaS1_arm')
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.05)
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
arm.set_planning_time(10)
# 控制机械臂回到初始化位置
arm.set_named_target('home')
arm.go()
# 移除场景中之前运行残留的物体
scene.remove_attached_object(end_effector_link, 'tool')
scene.remove_world_object('table')
scene.remove_world_object('target')
# 设置桌面的高度
table_ground = 0.4
# 设置table和tool的三维尺寸
table_size = [0.05, 0.4, 0.7] # 设置长宽高
tool_size = [0.2, 0.02, 0.02]
# 设置tool的位姿
p = PoseStamped()
#tool放置位置的参考坐标系
p.header.frame_id = end_effector_link
#tool的具体位置姿态
p.pose.position.x = tool_size[0] / 2.0 - 0.025
p.pose.position.y = -0.015
p.pose.position.z = 0.0
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1
# 将tool附着到机器人的终端，该函数为附着某物体到机器人上
scene.attach_box(end_effector_link, 'tool', p, tool_size,touch_links=[end_effector_link])
# 将table加入场景当中
table_pose = PoseStamped()
table_pose.header.frame_id = 'base_link'
table_pose.pose.position.x = 0.25
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box('table', table_pose, table_size) # 添加障碍物
rospy.sleep(1)
# 姿态使用四元数描述，基于base_link坐标系
target_pose = PoseStamped()
# 参考坐标系，前面设置了
target_pose.header.frame_id = 'base_link'
target_pose.header.stamp = rospy.Time.now() # 时间戳？
# 末端位置
target_pose.pose.position.x =0.424167
target_pose.pose.position.y = 0.0622007
target_pose.pose.position.z = 0.712836
# 末端姿态，四元数
target_pose.pose.orientation.x = 0.999735
target_pose.pose.orientation.y = 0.0230112
target_pose.pose.orientation.z = 0.0000697211
target_pose.pose.orientation.w = -0.000124006
# 更新当前的位姿
arm.set_start_state_to_current_state()
# 设置机械臂终端运动的目标位姿
arm.set_pose_target(target_pose, end_effector_link)
# 规划运动路径，返回虚影的效果
traj = arm.plan()
# 按照规划的运动路径控制机械臂运动
arm.execute(traj)
rospy.sleep(2) # 执行完成后休息1s
# 控制机械臂回到初始化位置
arm.set_named_target('home')
arm.go()
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
class MoveItDemo:
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
#Initialize robot
robot = moveit_commander.RobotCommander()
# Use the planning scene object to add or remove objects
self.scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10)
# Create a publisher for displaying object frames
self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10)
### Create a publisher for visualizing direction ###
self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the MoveIt! commander for the arm
self.right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the MoveIt! commander for the gripper
self.right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# eel = len(self.right_arm.get_end_effector_link())
# print eel
# Allow 5 seconds per planning attempt
# self.right_arm.set_planning_time(5)
# Prepare Action Controller for gripper
self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction)
self.ac.wait_for_server()
# Give the scene a chance to catch up
rospy.sleep(2)
# Prepare Gazebo Subscriber
self.pwh = None
self.pwh_copy = None
self.idx_targ = None
self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)
# self.m_error = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)
### OPEN THE GRIPPER ###
self.open_gripper()
### Attach / Remove Object ###
self.aro = None
# PREPARE THE SCENE
while self.pwh is None:
rospy.sleep(0.05)
# Run and keep in the BG the scene generator also add the ability to kill the code with ctrl^c
timerThread = threading.Thread(target=self.scene_generator)
timerThread.daemon = True
timerThread.start()
### GIVE SCENE TIME TO CATCH UP ###
rospy.sleep(5)
print "==================== Executing ==========================="
blist = ['target','custom_2','custom_3', 'custom_table']
self.idx_list = []
for name in obj_id:
print name
if name not in blist:
self.idx_list.append(obj_id.index(name))
################################## TESTING AREA #####################################
# ga = self.grasp_attempt()
# print ga
# print obj_id
success = False
i=0
while success == False:
idx = self.idx_list[i]
print idx
new_pose, ds = self.declutter_scene(idx)
if ds == True:
self.post_grasp(new_pose, obj_id[idx])
i+=1
################# GRASPING ATTEMPTS ###################
# ga = self.grasp_attempt()
# print ga
rospy.sleep(5)
# # Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# # Exit the script
moveit_commander.os._exit(0)
################################################################# FUNCTIONS #################################################################################
def grasp_attempt(self):
init_poses = []
grasp_poses = []
for axis in range(0,5):
pg = self.grasp_pose(obj_pose[obj_id.index('target')], axis, 'pg')
gp = self.grasp_pose(obj_pose[obj_id.index('target')], axis, 'gp')
init_poses.append(pg)
grasp_poses.append(gp)
gid, grasps = self.grasp_generator(grasp_poses)
for i in range(0, len(gid)):
self.gripper_pose_pub.publish(grasps[i])
rospy.sleep(0.1)
success = False
for pg in range(0,5):
print pg
plp = self.right_arm.plan(init_poses[pg].pose)
print len(plp.joint_trajectory.points)
if len(plp.joint_trajectory.points) == 0:
print "No valid pregrasp Position, continuing on next one"
continue
self.right_arm.plan(init_poses[pg].pose)
self.right_arm.go()
if pg == 0:
idx = gid.index('front')
elif pg == 1:
idx = gid.index('right')
elif pg == 2:
idx = gid.index('left')
elif pg == 3:
idx = gid.index('topx')
else:
idx = gid.index('topy')
print ("idx = ",idx)
for g in range(idx, len(gid)): ### TODO: fix the loop here, we dont want to check every single grasp after the index
print g
pl2 = self.right_arm.plan(grasps[g].pose)
print len(pl2.joint_trajectory.points)
if len(pl2.joint_trajectory.points) == 0:
print "No Valid Grasp, continuing on next one"
continue
self.right_arm.plan(grasps[g].pose)
self.right_arm.go()
success = True
break
if success == True:
break
return success
def declutter_scene(self,index):
print obj_id[index]
init_poses = []
grasp_poses = []
for axis in range(0,5):
pg = self.grasp_pose(obj_pose[index], axis, 'pg')
gp = self.grasp_pose(obj_pose[index], axis, 'gp')
init_poses.append(pg)
grasp_poses.append(gp)
gid, grasps = self.grasp_generator(grasp_poses)
for i in range(0, len(gid)):
self.gripper_pose_pub.publish(grasps[i])
rospy.sleep(0.1)
success = False
for pg in range(0,5):
print pg
plp = self.right_arm.plan(init_poses[pg].pose)
print len(plp.joint_trajectory.points)
if len(plp.joint_trajectory.points) == 0:
print "No valid pregrasp Position, continuing on next one"
continue
self.right_arm.plan(init_poses[pg].pose)
self.right_arm.go()
if pg == 0:
idx = gid.index('front')
elif pg == 1:
idx = gid.index('right')
elif pg == 2:
idx = gid.index('left')
elif pg == 3:
idx = gid.index('topx')
else:
idx = gid.index('topy')
print ("idx = ",idx)
for g in range(idx, len(gid)): ### TODO: fix the loop here, we dont want to check every single grasp after the index
print g
pl2 = self.right_arm.plan(grasps[g].pose)
print len(pl2.joint_trajectory.points)
if len(pl2.joint_trajectory.points) == 0:
print "No Valid Grasp, continuing on next one"
continue
self.right_arm.plan(grasps[g].pose)
self.right_arm.go()
new_pose = grasps[g]
success = True
break
if success == True:
break
return (new_pose,success)
def post_grasp(self,new_pose, obj_to_grasp):
######### GRASP OBJECT/ REMOVE FROM SCENE ######.
self.close_gripper()
self.aro = obj_to_grasp
print self.aro
# ### POST GRASP RETREAT ###
# M1 = transformations.quaternion_matrix([new_pose.pose.orientation.x, new_pose.pose.orientation.y, new_pose.pose.orientation.z, new_pose.pose.orientation.w])
# M1[0,3] = new_pose.pose.position.x
# M1[1,3] = new_pose.pose.position.y
# M1[2,3] = new_pose.pose.position.z
# M2 = transformations.euler_matrix(0, 0, 0)
# M2[0,3] = 0.0 # offset about x
# M2[1,3] = 0.0 # about y
# M2[2,3] = 0.3 # about z
# T1 = np.dot(M1, M2)
# npo = deepcopy(new_pose)
# npo.pose.position.x = T1[0,3]
# npo.pose.position.y = T1[1,3]
# npo.pose.position.z = T1[2,3]
# quat = transformations.quaternion_from_matrix(T1)
# npo.pose.orientation.x = quat[0]
# npo.pose.orientation.y = quat[1]
# npo.pose.orientation.z = quat[2]
# npo.pose.orientation.w = quat[3]
# npo.header.frame_id = REFERENCE_FRAME
# self.right_arm.plan(npo.pose)
# self.right_arm.go()
### PLACE THE OBJECT
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.80
place_pose.pose.position.y = -0.33
place_pose.pose.position.z = 0.53
place_pose.pose.orientation.w = 1.0
def grasp_pose(self, target_pose, axis, stage):
############ TODO : GENERATE AUTOMATED PRE-GRASPING POSITIONS BASED ON THE PRIMITIVE #########
if axis == 0:
M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M1[0,3] = target_pose.pose.position.x
M1[1,3] = target_pose.pose.position.y
M1[2,3] = target_pose.pose.position.z
M2 = transformations.euler_matrix(0, 0, 0)
if stage == 'pg':
M2[0,3] = -0.25 # offset about x
elif stage == 'gp':
M2[0,3] = -0.18 # offset about x
M2[1,3] = 0.0 # about y
M2[2,3] = 0.0 # about z
elif axis == 1:
M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M1[0,3] = target_pose.pose.position.x
M1[1,3] = target_pose.pose.position.y
M1[2,3] = target_pose.pose.position.z
M2 = transformations.euler_matrix(0, 0, 1.57)
M2[0,3] = 0.0 # offset about x
if stage == 'pg':
M2[1,3] = -0.25 # about y
elif stage == 'gp':
M2[1,3] = -0.18 # about y
M2[2,3] = 0.0 # about z
elif axis ==2:
M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M1[0,3] = target_pose.pose.position.x
M1[1,3] = target_pose.pose.position.y
M1[2,3] = target_pose.pose.position.z
M2 = transformations.euler_matrix(0, 0, -1.57)
M2[0,3] = 0.0 # offset about x
if stage == 'pg':
M2[1,3] = 0.25 # about y
elif stage == 'gp':
M2[1,3] = 0.18 # about y
M2[2,3] = 0.0 # about z
elif axis ==3:
M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M1[0,3] = target_pose.pose.position.x
M1[1,3] = target_pose.pose.position.y
M1[2,3] = target_pose.pose.position.z
M2 = transformations.euler_matrix(0, 1.57, 0)
M2[0,3] = 0.0 # offset about x
M2[1,3] = 0.0 # about y
if stage == 'pg':
M2[2,3] = 0.30 # about z
elif stage == 'gp':
M2[2,3] = 0.23 # about z
else:
M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M1[0,3] = target_pose.pose.position.x
M1[1,3] = target_pose.pose.position.y
M1[2,3] = target_pose.pose.position.z
M2 = transformations.euler_matrix(1.57, 1.57, 0)
M2[0,3] = 0.0 # offset about x
M2[1,3] = 0.0 # about y
if stage == 'pg':
M2[2,3] = 0.30 # about z
elif stage == 'gp':
M2[2,3] = 0.23 # about z
T1 = np.dot(M1, M2)
grasp_pose = deepcopy(target_pose)
grasp_pose.pose.position.x = T1[0,3]
grasp_pose.pose.position.y = T1[1,3]
grasp_pose.pose.position.z = T1[2,3]
quat = transformations.quaternion_from_matrix(T1)
grasp_pose.pose.orientation.x = quat[0]
grasp_pose.pose.orientation.y = quat[1]
grasp_pose.pose.orientation.z = quat[2]
grasp_pose.pose.orientation.w = quat[3]
grasp_pose.header.frame_id = REFERENCE_FRAME
return grasp_pose
def grasp_generator(self, initial_poses):
# A list to hold the grasps
grasps = []
o = [] # Original Pose of the object (o)
O=[]
gid = []
i= 0
while i < len(initial_poses):
o.append(initial_poses[i])
i+=1
G = transformations.euler_matrix(0, 0, 0)
# Generate a grasps for along z axis (x and y directions)
k = 0
while k <= 4:
O.append(transformations.quaternion_matrix([o[k].pose.orientation.x, o[k].pose.orientation.y, o[k].pose.orientation.z, o[k].pose.orientation.w]))
O[k][0,3] = o[k].pose.position.x
O[k][1,3] = o[k].pose.position.y
O[k][2,3] = o[k].pose.position.z
if k in range(0,3):
for z in self.drange(-obj_size[obj_id.index('target')][2]/2, obj_size[obj_id.index('target')][2]/2, 0.02):
# print z
T = np.dot(O[k], G)
grasp = deepcopy(o[k])
grasp.pose.position.x = T[0,3]
grasp.pose.position.y = T[1,3]
grasp.pose.position.z = T[2,3] +z
quat = transformations.quaternion_from_matrix(T)
grasp.pose.orientation.x = quat[0]
grasp.pose.orientation.y = quat[1]
grasp.pose.orientation.z = quat[2]
grasp.pose.orientation.w = quat[3]
grasp.header.frame_id = REFERENCE_FRAME
# Append the grasp to the list
grasps.append(deepcopy(grasp))
if k ==0:
gid.append('front')
elif k ==1:
gid.append('right')
elif k ==2:
gid.append('left')
elif k == 3:
for x in self.drange(-obj_size[obj_id.index('target')][1]/2, obj_size[obj_id.index('target')][1]/2, 0.01):
# print z
T = np.dot(O[k], G)
grasp = deepcopy(o[k])
grasp.pose.position.x = T[0,3] +x
grasp.pose.position.y = T[1,3]
grasp.pose.position.z = T[2,3]
quat = transformations.quaternion_from_matrix(T)
grasp.pose.orientation.x = quat[0]
grasp.pose.orientation.y = quat[1]
grasp.pose.orientation.z = quat[2]
grasp.pose.orientation.w = quat[3]
grasp.header.frame_id = REFERENCE_FRAME
# Append the grasp to the list
grasps.append(deepcopy(grasp))
gid.append('topx')
else:
for y in self.drange(-obj_size[obj_id.index('target')][1]/2, obj_size[obj_id.index('target')][1]/2, 0.01):
# print z
T = np.dot(O[k], G)
grasp = deepcopy(o[k])
grasp.pose.position.x = T[0,3]
grasp.pose.position.y = T[1,3] +y
grasp.pose.position.z = T[2,3]
quat = transformations.quaternion_from_matrix(T)
grasp.pose.orientation.x = quat[0]
grasp.pose.orientation.y = quat[1]
grasp.pose.orientation.z = quat[2]
grasp.pose.orientation.w = quat[3]
grasp.header.frame_id = REFERENCE_FRAME
# Append the grasp to the list
grasps.append(deepcopy(grasp))
gid.append('topy')
k+=1
# Return the list
return (gid,grasps)
def scene_generator(self):
obj_pose =[]
obj_id = []
obj_size = []
bl = ['ground_plane','pr2']
global obj_pose, obj_id , obj_size
ops = PoseStamped()
ops.header.frame_id = REFERENCE_FRAME
for model_name in self.pwh.name:
if model_name not in bl:
obj_id.append(model_name)
ops.pose = self.pwh.pose[self.pwh.name.index(model_name)]
obj_pose.append(deepcopy(ops))
obj_size.append([0.05, 0.05, 0.15])
obj_id[obj_id.index('custom_1')] = 'target'
obj_size[obj_id.index('custom_2')] = [0.05, 0.05, 0.10]
obj_size[obj_id.index('custom_3')] = [0.05, 0.05, 0.05]
obj_size[obj_id.index('custom_table')] = [1.5, 0.8, 0.03]
### REMOVE OBJECT FROM PREVIOUS RUNS ###
for i in range(0, len(obj_id)):
self.scene.remove_world_object(obj_id[i])
# Remove any attached objects from a previous session
self.scene.remove_attached_object(GRIPPER_FRAME, obj_id[obj_id.index('target')])
next_call = time.time()
while True:
next_call = next_call+1
if self.aro is None:
for i in range(0, len(obj_id)):
### CREATE THE SCENE ###
self.scene.add_box(obj_id[i], obj_pose[i], obj_size[i])
### Make the target purple and obstacles orange ###
self.setColor(obj_id[obj_id.index('target')], 0.6, 0, 1, 1.0)
self.setColor(obj_id[obj_id.index('custom_2')], 1, 0.623, 0, 1.0)
self.setColor(obj_id[obj_id.index('custom_3')], 1, 0.623, 0, 1.0)
self.setColor(obj_id[obj_id.index('custom_1_0')], 1, 0.623, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
else:
###### ATTACH ITEM TO GRIPPER ######
touch_links = [GRIPPER_FRAME, 'r_gripper_l_finger_tip_link','r_gripper_r_finger_tip_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link']
#print touch_links
self.scene.attach_box(GRIPPER_FRAME, obj_id[self.aro], obj_pose[self.aro], obj_size[self.aro], touch_links)
### REMOVE SPECIFIC OBJECT AFTER IT HAS BEEN ATTACHED TO GRIPPER ###
self.scene.remove_world_object(obj_id[self.aro])
time.sleep(next_call - time.time())
def model_state_callback(self,msg):
self.pwh = ModelStates()
self.pwh = msg
def drange(self, start, stop, step):
r = start
while r < stop:
yield r
r += step
def close_gripper(self):
g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.035, 100))
self.ac.send_goal(g_close)
self.ac.wait_for_result()
rospy.sleep(15) # Gazebo requires up to 15 seconds to attach object
def open_gripper(self):
g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100))
self.ac.send_goal(g_open)
self.ac.wait_for_result()
rospy.sleep(5) # And up to 20 to detach it
# Set the color of an object
def setColor(self, name, r, g, b, a = 0.9):
# Initialize a MoveIt color object
color = ObjectColor()
# Set the id to the name given as an argument
color.id = name
# Set the rgb and alpha values given as input
color.color.r = r
color.color.g = g
color.color.b = b
color.color.a = a
# Update the global color dictionary
self.colors[name] = color
# Actually send the colors to MoveIt!
def sendColors(self):
# Initialize a planning scene object
p = PlanningScene()
# Need to publish a planning scene diff
p.is_diff = True
# Append the colors from the global color dictionary
for color in self.colors.values():
p.object_colors.append(color)
# Publish the scene diff
self.scene_pub.publish(p)
class PickAndPlace(object):
def __init__(self, limb, start_pose, hover_distance=0.15):
self.start_pose = start_pose
self.hover_distance = hover_distance
self.limb = robot_limb.Limb(limb)
self.gripper = robot_gripper.Gripper(limb)
self.base = "base_marker"
self.pick_obj = "obj_marker"
self.dest = "dest_marker"
self.destinations = []
self.queue = Queue.Queue()
self.tf = TransformListener()
self.set_scene()
self.set_limb_planner()
self.enable_robot()
self.set_destinations()
print("Moving Sawyer Arm to Start Position")
self.move_to_start()
def set_limb_planner():
self.right_arm = MoveGroupCommander("right_arm")
self.right_arm.set_planner_id('RRTConnectkConfigDefault')
self.right_arm.allow_replanning(True)
self.right_arm.set_planning_time(7)
def enable_robot():
print("Getting robot state... ")
self._rs = intera_interface.RobotEnable(intera_interface.CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
def set_scene():
self.scene_pub = rospy.Publisher('/planning_scene',
PlanningScene,
queue_size=10)
self.scene = PlanningSceneInterface()
rospy.sleep(2)
self.add_collision_object('turtlebot', 0.8, 0., -0.65, .5, 1.5, .33)
self.add_collision_object('right_wall', 0., 0.65, 0., 4., .1, 4.)
self.add_collision_object('left_wall', 0., -0.8, 0., 4., .1, 4.)
self.add_collision_object('back_wall', -0.6, 0., 0., .1, 4., 4.)
self.add_collision_object('destination_table', .5, -.4, -.35, 1, .5,
.5)
self.plan_scene = PlanningScene()
self.plan_scene.is_diff = True
self.scene_pub.publish(self.plan_scene)
def add_collision_object(self, name, x, y, z, xs, ys, zs):
self.scene.remove_world_object(name)
o = PoseStamped()
o.pose.position.x = x
o.pose.position.y = y
o.pose.position.z = z
self.scene.attach_box('base', name, o, [xs, ys, zs])
def set_destinations(self):
while len(self.destinations) == 0:
if self.tf.frameExists(self.base) and self.tf.frameExists(
self.dest):
point, quaternion = self.tf.lookupTransform(
self.base, self.dest,
self.tf.getLatestCommonTime(self.base, self.dest))
position = list_to_point(point)
self.destinations.append(
Pose(position=position,
orientation=self.start_pose.orientation))
def add_new_objects_to_queue(self, sleep=True):
print("Checking if " + self.base + " and " + self.pick_obj +
" both exist.")
if self.tf.frameExists(self.base) and self.tf.frameExists(
self.pick_obj):
if sleep:
rospy.sleep(10.0)
self.add_new_objects_to_queue(sleep=False)
else:
print(self.base + " and " + self.pick_obj + " both exist.")
point, quaternion = self.tf.lookupTransform(
self.base, self.pick_obj,
self.tf.getLatestCommonTime(self.base, self.pick_obj))
position = list_to_point(point)
print("Adding " + self.pick_obj + " to queue")
obj_location = Pose(position=position,
orientation=self.start_pose.orientation)
print("Picking Object from:", obj_location)
self.queue.put(obj_location)
def complete_pick_place(self):
if not self.queue.empty():
start_pose = self.queue.get()
end_pose = self.destinations[0]
print("\nPicking...")
self.pick(start_pose)
print("\nPlacing...")
self.place(end_pose)
print("\Resetting...")
self.move_to_start()
def guarded_move_to_joint_position(self, pose):
self.right_arm.set_pose_target(pose)
self.right_arm.go()
def servo_to_pose(self, pose):
# servo down to release
self.guarded_move_to_joint_position(pose)
def approach(self, pose):
# approach with a pose the hover-distance above the requested pose
approach = copy.deepcopy(pose)
approach.position.z += self.hover_distance
print("approaching:", approach)
self.guarded_move_to_joint_position(approach)
def retract(self):
# retrieve current pose from endpoint
current_pose = self.limb.endpoint_pose()
ik_pose = Pose()
ik_pose.position.x = current_pose['position'].x
ik_pose.position.y = current_pose['position'].y
ik_pose.position.z = current_pose['position'].z + self.hover_distance
ik_pose.orientation.x = current_pose['orientation'].x
ik_pose.orientation.y = current_pose['orientation'].y
ik_pose.orientation.z = current_pose['orientation'].z
ik_pose.orientation.w = current_pose['orientation'].w
# servo up from current pose
self.guarded_move_to_joint_position(ik_pose)
def move_to_start(self):
print("Moving the right arm to start pose...")
self.guarded_move_to_joint_position(self.start_pose)
self.gripper_open()
rospy.sleep(1.0)
print("Running. Ctrl-c to quit")
def gripper_open(self):
self.gripper.open()
rospy.sleep(1.0)
def gripper_close(self):
self.gripper.close()
rospy.sleep(1.0)
def pick(self, pose):
# open the gripper
self.gripper_open()
# servo above pose
self.approach(pose)
# servo to pose
self.servo_to_pose(pose)
# close gripper
self.gripper_close()
# retract to clear object
self.retract()
def place(self, pose):
# servo above pose
self.approach(pose)
# servo to pose
self.servo_to_pose(pose)
# open the gripper
self.gripper_open()
# retract to clear object
self.retract()
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Construct the initial scene object
scene = PlanningSceneInterface()
# Pause for the scene to get ready
rospy.sleep(1)
# Initialize the MoveIt! commander for the right arm
right_arm = MoveGroupCommander('right_arm')
# Initialize the MoveIt! commander for the gripper
right_gripper = MoveGroupCommander('right_gripper')
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.01)
right_arm.set_goal_orientation_tolerance(0.05)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Allow 5 seconds per planning attempt
right_arm.set_planning_time(5)
# Remove leftover objects from a previous run
scene.remove_attached_object(end_effector_link, 'tool')
scene.remove_world_object('table')
scene.remove_world_object('box1')
scene.remove_world_object('box2')
scene.remove_world_object('target')
# Set the height of the table off the ground
table_ground = 0.75
# Set the length, width and height of the table
table_size = [0.2, 0.7, 0.01]
# Set the length, width and height of the object to attach
tool_size = [0.3, 0.02, 0.02]
# Create a pose for the tool relative to the end-effector
p = PoseStamped()
p.header.frame_id = end_effector_link
scene.attach_mesh
# Place the end of the object within the grasp of the gripper
p.pose.position.x = tool_size[0] / 2.0 - 0.025
p.pose.position.y = 0.0
p.pose.position.z = 0.0
# Align the object with the gripper (straight out)
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1
# Attach the tool to the end-effector
scene.attach_box(end_effector_link, 'tool', p, tool_size)
# Add a floating table top
table_pose = PoseStamped()
table_pose.header.frame_id = 'base_footprint'
table_pose.pose.position.x = 0.35
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box('table', table_pose, table_size)
# Update the current state
right_arm.set_start_state_to_current_state()
# Move the arm with the attached object to the 'straight_forward' position
right_arm.set_named_target('straight_forward')
right_arm.go()
rospy.sleep(2)
# Return the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('resting')
right_arm.go()
rospy.sleep(2)
scene.remove_attached_object(end_effector_link, 'tool')
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def simple_function():
rc = RobotCommander()
mgc = MoveGroupCommander("manipulator")
# print(rc.get_group_names())
# print(rc.get_group('manipulator'))
# exit()
eef_step = 0.01
jump_threshold = 2
### Create a handle for the Planning Scene Interface
psi = PlanningSceneInterface()
rc.get_current_state()
rospy.logwarn(rc.get_current_state())
sss.wait_for_input()
#rate = rospy.Rate(0.1) # 10hz
rate = rospy.Rate(1) # 10hz
rospy.sleep(1)
theSub = rospy.Subscriber('/attached_collision_object', AttachedCollisionObject, attCollObjCb, queue_size = 1)
while not rospy.is_shutdown():
approach_pose = PoseStamped()
approach_pose.header.frame_id = "table"
approach_pose.header.stamp = rospy.Time(0)
approach_pose.pose.position.x = 0.146
approach_pose.pose.position.y = 0.622
approach_pose.pose.position.z = 0.01
quat = tf.transformations.quaternion_from_euler(0, 0, 1.57/2)
approach_pose.pose.orientation.x = quat[0]
approach_pose.pose.orientation.y = quat[1]
approach_pose.pose.orientation.z = quat[2]
approach_pose.pose.orientation.w = quat[3]
# mgc.set_start_state_to_current_state()
# (traj_approach,frac_approach) = mgc.compute_cartesian_path([approach_pose.pose], eef_step, jump_threshold, True)
# if(frac_approach != 1):
# rospy.logwarn("Planning did not succeed before adding collision objects")
# else:
# rospy.logwarn("Planning succeeded before adding collision objects")
#
# rospy.logwarn("waiting for input to add dummy box")
# sss.wait_for_input()
#
box_dummy_pose = PoseStamped()
box_dummy_pose.header.frame_id = "table"
box_dummy_pose.pose.position.x = 0.147
box_dummy_pose.pose.position.y = 0.636
box_dummy_pose.pose.position.z = 0
psi.add_box("dummy_box", box_dummy_pose, (0.18,0.09,0.26))# #size x,y,z x is always to the left viewing the robot from the PC
# rospy.logwarn("I have added the box")
# rospy.sleep(1)
# rospy.logwarn("waiting for input to try planning with dummy box")
# sss.wait_for_input()
#
# (traj_approach,frac_approach) = mgc.compute_cartesian_path([approach_pose.pose], eef_step, jump_threshold, True)
# if(frac_approach != 1):
# rospy.logwarn("Planning did not succeed after adding collision objects (dummy box)")
# else:
# rospy.logwarn("Planning succeeded after adding collision objects (dummy box)")
#
rospy.logwarn("waiting for input to add end effector box box")
sss.wait_for_input()
# end effector collision object
link_attached_to_ef = "ee_link"
mb_ef_collisonobj = "metal_bracket"
mb_ef_pose = PoseStamped()
mb_ef_pose.header.frame_id = link_attached_to_ef
mb_ef_pose.pose.position.x = 0.065/2.0
mb_ef_pose.pose.position.y = 0.0
mb_ef_pose.pose.position.z = 0.0
mb_ef_size = (0.065,0.06,0.06)
psi.attach_box(link_attached_to_ef, mb_ef_collisonobj, mb_ef_pose, mb_ef_size, [link_attached_to_ef, "wrist_3_link"])
raw_input("0 hi")
mgc.attach_object("dummy_box", link_attached_to_ef, [link_attached_to_ef, "wrist_3_link"])
rospy.logwarn("I have added the attached box to the end effector")
rospy.sleep(1)
raw_input("1 hi")
rospy.logwarn(rc.get_current_state())
# mgc.attach_object(object_name, link_name, touch_links)
mgc.set_start_state_to_current_state()
rospy.logwarn(rc.get_current_state())
raw_input("2 hi")
rospy.logwarn("waiting for input to try planning with end effector box")
sss.wait_for_input()
(traj_approach,frac_approach) = mgc.compute_cartesian_path([approach_pose.pose], eef_step, jump_threshold, True)
if(frac_approach != 1):
rospy.logwarn("Planning did not succeed after adding collision objects (dummy box)")
else:
rospy.logwarn("Planning succeeded after adding collision objects (dummy box)")
rospy.logwarn("waiting for input to try planning next loop")
sss.wait_for_input()
rate.sleep()
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
scene = PlanningSceneInterface()
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
self.colors = dict()
rospy.sleep(1)
arm = MoveGroupCommander('arm')
gripper = MoveGroupCommander('gripper')
end_effector_link = arm.get_end_effector_link()
arm.set_goal_position_tolerance(0.005)
arm.set_goal_orientation_tolerance(0.025)
arm.allow_replanning(True)
gripper.set_goal_position_tolerance(0.005)
gripper.set_goal_orientation_tolerance(0.025)
gripper.allow_replanning(True)
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
arm.set_planning_time(5)
r_tool_size = [0.03, 0.01, 0.06]
l_tool_size = [0.03, 0.01, 0.06]
#left gripper
l_p = PoseStamped()
l_p.header.frame_id = end_effector_link
l_p.pose.position.x = 0.00
l_p.pose.position.y = 0.04
l_p.pose.position.z = 0.04
l_p.pose.orientation.w = 1
scene.attach_box(end_effector_link, 'l_tool', l_p, l_tool_size)
#right gripper
r_p = PoseStamped()
r_p.header.frame_id = end_effector_link
r_p.pose.position.x = 0.00
r_p.pose.position.y = -0.04
r_p.pose.position.z = 0.04
r_p.pose.orientation.w = 1
scene.attach_box(end_effector_link, 'r_tool', r_p, r_tool_size)
grasp_pose = target_pose
grasp_pose.pose.position.x -= 0.15
#grasp_pose.pose.position.z=
grasp_pose.pose.orientation.x = 0
grasp_pose.pose.orientation.y = 0.707
grasp_pose.pose.orientation.z = 0
grasp_pose.pose.orientation.w = 0.707
arm.set_start_state_to_current_state()
arm.set_pose_target(grasp_pose, end_effector_link)
traj = arm.plan()
arm.execute(traj)
rospy.sleep(2)
#arm.shift_pose_target(4,1.57,end_effector_link)
#arm.go()
#rospy.sleep(2)
arm.shift_pose_target(0, 0.11, end_effector_link)
arm.go()
rospy.sleep(2)
saved_target_pose = arm.get_current_pose(end_effector_link)
#arm.set_named_target("initial_arm2")
#grasp
scene.attach_box(end_effector_link, target_id, g_p, target_size)
rospy.sleep(2)
#grasping is over , from now is placing
arm.shift_pose_target(2, 0.15, end_effector_link)
arm.go()
rospy.sleep(2)
arm.shift_pose_target(1, -0.2, end_effector_link)
arm.go()
rospy.sleep(2)
#pouring test
arm.shift_pose_target(3, 1.57, end_effector_link)
rospy.sleep(2)
arm.shift_pose_target(3, -1.57, end_effector_link)
rospy.sleep(2)
#gripper.shift_pose_target(3,1.57,end_effector_link)
arm.set_pose_target(saved_target_pose)
arm.go()
rospy.sleep(2)
scene.remove_attached_object(end_effector_link, 'l_tool')
scene.remove_attached_object(end_effector_link, 'r_tool')
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_attached_object_demo')
# 初始化场景对象
scene = PlanningSceneInterface()
rospy.sleep(1)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('manipulator')
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.05)
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 控制机械臂回到初始化位置
arm.set_named_target('home')
arm.go()
# 设置每次运动规划的时间限制：10s
arm.set_planning_time(10)
# 移除场景中之前运行残留的物体
scene.remove_attached_object(end_effector_link, 'tool')
scene.remove_world_object('table')
# 设置桌面的高度
table_ground = 0.7
# 设置table和tool的三维尺寸
table_size = [0.1, 0.3, 0.02]
tool_size = [0.2, 0.02, 0.02]
# 设置tool的位姿
p = PoseStamped()
p.header.frame_id = end_effector_link
p.pose.position.x = tool_size[0] / 2.0
p.pose.position.z = -0.015
p.pose.orientation.w = 1
# 将tool附着到机器人的终端
scene.attach_box(end_effector_link, 'tool', p, tool_size)
# 将table加入场景当中
table_pose = PoseStamped()
table_pose.header.frame_id = 'base_link'
table_pose.pose.position.x = 0.4
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2]
table_pose.pose.orientation.w = 1.0
scene.add_box('table', table_pose, table_size)
rospy.sleep(2)
# 更新当前的位姿
arm.set_start_state_to_current_state()
# 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
joint_positions = [
-0.5110620856285095, 0.32814791798591614, 0.5454912781715393,
1.0146701335906982, 0.8498637080192566, -0.45695754885673523
]
arm.set_joint_value_target(joint_positions)
# 控制机械臂完成运动
arm.go()
rospy.sleep(1)
# 控制机械臂回到初始化位置
arm.set_named_target('home')
arm.go()
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def main(argv):
#moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node("PickPlaceDemo")
rospy.on_shutdown(onshutdownhook)
transform = tf.TransformListener()
global scene
scene = PlanningSceneInterface()
robot = RobotCommander()
group = MoveGroupCommander("manipulator")
rospy.sleep(2)
#scene.is_diff = True
#t = transform.getLatestCommonTime("/tool0", "/world")
#tool0_pose, tool0_quaternion = transform.lookupTransform("world", "tool0", t)
quaternion = tf.transformations.quaternion_from_euler(0.0, 0.0, 0.0)
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = -0.700
p.pose.position.y = 0.000
p.pose.position.z = 0.060
p.pose.orientation.x = quaternion[0]
p.pose.orientation.y = quaternion[1]
p.pose.orientation.z = quaternion[2]
p.pose.orientation.w = quaternion[3]
scene.add_box("box1", p, (0.15, 0.15, 0.15))
p.pose.position.x = -0.700
p.pose.position.y = -0.250
p.pose.position.z = 0.060
scene.add_box("box2", p, (0.15, 0.15, 0.15))
p.pose.position.x = -0.700
p.pose.position.y = -0.500
p.pose.position.z = 0.060
scene.add_box("box3", p, (0.15, 0.15, 0.15))
rospy.sleep(5)
quaternion = tf.transformations.quaternion_from_euler(
0.0, (math.pi / 2.0), 0.0)
pose_target = Pose()
pose_target.position.x = -0.700
pose_target.position.y = 0.000
pose_target.position.z = 0.155
pose_target.orientation.x = quaternion[0]
pose_target.orientation.y = quaternion[1]
pose_target.orientation.z = quaternion[2]
pose_target.orientation.w = quaternion[3]
group.set_pose_target(pose_target)
group.go(wait=True)
rospy.sleep(1)
scene.attach_box("tool0", "box1")
rospy.sleep(1)
pose_target.position.x = 0.000
pose_target.position.y = -0.700
pose_target.position.z = 0.155
group.set_pose_target(pose_target)
group.go(wait=True)
rospy.sleep(1)
scene.remove_attached_object("tool0")
rospy.sleep(1)
pose_target.position.x = -0.700
pose_target.position.y = -0.250
pose_target.position.z = 0.155
group.set_pose_target(pose_target)
group.go(wait=True)
rospy.sleep(1)
scene.attach_box("tool0", "box2")
rospy.sleep(1)
pose_target.position.x = 0.000
pose_target.position.y = -0.700
pose_target.position.z = 0.306
group.set_pose_target(pose_target)
group.go(wait=True)
rospy.sleep(1)
scene.remove_attached_object("tool0")
rospy.sleep(1)
pose_target.position.x = -0.700
pose_target.position.y = -0.500
pose_target.position.z = 0.155
group.set_pose_target(pose_target)
group.go(wait=True)
rospy.sleep(1)
scene.attach_box("tool0", "box3")
rospy.sleep(1)
pose_target.position.x = 0.000
pose_target.position.y = -0.700
pose_target.position.z = 0.457
group.set_pose_target(pose_target)
group.go(wait=True)
rospy.sleep(1)
scene.remove_attached_object("tool0")
while not rospy.is_shutdown():
pass
class PlanningSceneHelper:
def __init__(self, package=None, mesh_folder_path=None):
# interface to the planning and collision scenes
self.psi = PlanningSceneInterface()
# self.scene_pub = Publisher("/collision_object", CollisionObject,
# queue_size=1, latch=True)
self.vis_pub = Publisher("/collision_scene",
MarkerArray,
queue_size=10)
self.marker_array = MarkerArray()
sleep(1.0)
if package is not None and mesh_folder_path is not None:
# Build folder path for use in load
rospack = RosPack()
self.package = package
self.mesh_folder_path = mesh_folder_path
self.package_path = rospack.get_path(package)
self.folder_path = os.path.join(self.package_path, mesh_folder_path)\
+ os.sep
else:
loginfo(
"Missing package or mesh folder path supplied to planning_scene_helper; no meshes can be added"
)
# Create dict of objects, for removing markers later
self.objects = {}
self.next_id = 1
# Loads the selected mesh file into collision scene at the desired location.
def add_mesh(self,
object_id,
frame,
filename,
visual=None,
pose=None,
position=None,
orientation=None,
rpy=None,
color=None):
if self.package is None:
logwarn("No package was provided; no meshes can be loaded.")
if visual is None:
visual = filename
filename = self.folder_path + filename
stamped_pose = self.make_stamped_pose(frame=frame,
pose=pose,
position=position,
orientation=orientation,
rpy=rpy)
try:
self.psi.add_mesh(object_id, stamped_pose, filename)
loginfo("Loaded " + object_id + " from" + filename +
" as collision object.")
marker = self.make_marker_stub(stamped_pose, color=color)
marker.type = Marker.MESH_RESOURCE
# marker.mesh_resource = "file:/" + self.folder_path + visual
marker.mesh_resource = "package://" + self.package + os.sep + self.mesh_folder_path + os.sep + visual
self.marker_array = self.make_new_marker_array_msg()
self.marker_array.markers.append(marker)
self.vis_pub.publish(self.marker_array)
self.objects[object_id] = marker
loginfo("Loaded " + object_id + " from " + marker.mesh_resource +
" as marker.")
except ROSException as e:
loginfo("Problem loading " + object_id + " collision object: " +
str(e))
except AssimpError as ae:
loginfo("Problem loading " + object_id + " collision object: " +
str(ae))
def add_cylinder(self,
object_id,
frame,
size,
pose=None,
position=None,
orientation=None,
rpy=None,
color=None):
try:
stamped_pose = self.make_stamped_pose(frame=frame,
pose=pose,
position=position,
orientation=orientation,
rpy=rpy)
# Cylinders are special - they are not supported directly by
# moveit_commander. It must be published manually to collision scene
cyl = CollisionObject()
cyl.operation = CollisionObject.ADD
cyl.id = object_id
cyl.header = stamped_pose.header
prim = SolidPrimitive()
prim.type = SolidPrimitive.CYLINDER
prim.dimensions = [size[0], size[1]]
cyl.primitives = [prim]
cyl.primitive_poses = [stamped_pose.pose]
# self.scene_pub.publish(cyl)
marker = self.make_marker_stub(stamped_pose,
[size[1] * 2, size[2] * 2, size[0]],
color=color)
marker.type = Marker.CYLINDER
self.publish_marker(object_id, marker)
sleep(0.5)
logdebug("Loaded " + object_id +
" as cylindrical collision object.")
except ROSException as e:
loginfo("Problem loading " + object_id + " collision object: " +
str(e))
def add_sphere(self,
object_id,
frame,
size,
pose=None,
position=None,
orientation=None,
rpy=None,
color=None):
try:
stamped_pose = self.make_stamped_pose(frame=frame,
pose=pose,
position=position,
orientation=orientation,
rpy=rpy)
self.psi.add_sphere(object_id, stamped_pose, size[0])
loginfo("got past adding collision scene object")
marker = self.make_marker_stub(
stamped_pose, [size[0] * 2, size[1] * 2, size[2] * 2],
color=color)
marker.type = Marker.SPHERE
self.publish_marker(object_id, marker)
sleep(0.5)
loginfo("Loaded " + object_id + " as collision object.")
except ROSException as e:
loginfo("Problem loading " + object_id + " collision object: " +
str(e))
def add_box(self,
object_id,
frame,
size,
pose=None,
position=None,
orientation=None,
rpy=None,
color=None):
try:
stamped_pose = self.make_stamped_pose(frame=frame,
pose=pose,
position=position,
orientation=orientation,
rpy=rpy)
self.psi.add_box(object_id, stamped_pose, size)
marker = self.make_marker_stub(stamped_pose, size, color=color)
marker.type = Marker.CUBE
self.publish_marker(object_id, marker)
sleep(0.5)
loginfo("Loaded " + object_id + " as collision object.")
except ROSException as e:
loginfo("Problem loading " + object_id + " collision object: " +
str(e))
def attach_box(self,
link,
object_id,
frame,
size,
attach_to_link,
pose=None,
position=None,
orientation=None,
rpy=None):
"""TODO: color is not yet supported, since it's not internally
supported by psi.attach_box. Basically duplicate this method
but with color support."""
try:
stamped_pose = self.make_stamped_pose(frame=frame,
pose=pose,
position=position,
orientation=orientation,
rpy=rpy),
self.psi.attach_box(link, object_id, stamped_pose, size,
attach_to_link)
sleep(0.5)
loginfo("Attached " + object_id + " as collision object.")
except ROSException as e:
loginfo("Problem attaching " + object_id + " collision object: " +
str(e))
@staticmethod
def make_stamped_pose(frame,
pose=None,
position=None,
orientation=None,
rpy=None):
if orientation is not None and rpy is not None:
logwarn("Collision object has both orientation (quaternion) and "
"Rotation (rpy) defined! Defaulting to quaternion "
"representation")
stamped_pose = PoseStamped()
stamped_pose.header.frame_id = frame
stamped_pose.header.stamp = Time.now()
# use a pose if one is provided, otherwise, make your own from the
# position and orientation.
if pose is not None:
stamped_pose.pose = pose
else:
stamped_pose.pose.position.x = position[0]
stamped_pose.pose.position.y = position[1]
stamped_pose.pose.position.z = position[2]
# for orientation, allow either quaternion or RPY specification
if orientation is not None:
stamped_pose.pose.orientation.x = orientation[0]
stamped_pose.pose.orientation.y = orientation[1]
stamped_pose.pose.orientation.z = orientation[2]
stamped_pose.pose.orientation.w = orientation[3]
elif rpy is not None:
quaternion = transformations.quaternion_from_euler(
rpy[0], rpy[1], rpy[2])
stamped_pose.pose.orientation.x = quaternion[0]
stamped_pose.pose.orientation.y = quaternion[1]
stamped_pose.pose.orientation.z = quaternion[2]
stamped_pose.pose.orientation.w = quaternion[3]
else:
stamped_pose.pose.orientation.w = 1 # make basic quaternion
return stamped_pose
# Fill a ROS Marker message with the provided data
def make_marker_stub(self, stamped_pose, size=None, color=None):
if color is None:
color = (0.5, 0.5, 0.5, 1.0)
if size is None:
size = (1, 1, 1)
marker = Marker()
marker.header = stamped_pose.header
# marker.ns = "collision_scene"
marker.id = self.next_id
marker.lifetime = Time(0) # forever
marker.action = Marker.ADD
marker.pose = stamped_pose.pose
marker.color.r = color[0]
marker.color.g = color[1]
marker.color.b = color[2]
if len(color) == 4:
alpha = color[3]
else:
alpha = 1.0
marker.color.a = alpha
marker.scale.x = size[0]
marker.scale.y = size[1]
marker.scale.z = size[2]
self.next_id += 1
return marker
def make_new_marker_array_msg(self):
ma = MarkerArray()
return ma
# publish a single marker
def publish_marker(self, object_id, marker):
loginfo("Publishing marker for object {}".format(object_id))
self.marker_array = self.make_new_marker_array_msg()
self.marker_array.markers.append(marker)
self.vis_pub.publish(self.marker_array)
self.objects[object_id] = marker
# Remove the provided object from the world.
def remove(self, object_id):
# if object_id not in self.objects:
# logwarn("PlanningSceneHelper was not used to create object with id "
# + object_id + ". Attempting to remove it anyway.")
try:
# first remove the actual collision object
self.psi.remove_world_object(object_id)
if object_id in self.objects:
# then remove the marker associated with it
marker = self.objects[object_id]
marker.action = Marker.DELETE
self.publish_marker(object_id, marker)
self.objects.pop(object_id)
logdebug("Marker for collision object " + object_id + " removed.")
except ROSException as e:
loginfo("Problem removing " + object_id +
" from collision scene:" + str(e))
return
except KeyError as e:
loginfo("Problem removing " + object_id +
" from collision scene:" + str(e))
return
loginfo("Model " + object_id + " removed from collision scene.")
# Remove the provided attached collision object.
def remove_attached(self, link, object_id):
try:
self.psi.remove_attached_object(link=link, name=object_id)
loginfo("Attached object '" + object_id +
"' removed from collision scene.")
except:
loginfo("Problem attached object '" + object_id +
"' removing from collision scene.")
class MoveitObjectHandler(object):
'''
Use this class to create objects that reside in moveit environments
'''
def __init__(self):
'''
Constructor of the MoveitObjectHandler class.
'''
self.planning_scene_interface = PlanningSceneInterface()
rospy.loginfo("Connecting to /get_planning_scene service")
self.scene_srv = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene)
self.scene_srv.wait_for_service()
rospy.loginfo("Connected.")
rospy.loginfo("Connecting to clear octomap service...")
self.clear_octomap_srv = rospy.ServiceProxy(
'/clear_octomap', Empty)
self.clear_octomap_srv.wait_for_service()
rospy.loginfo("Connected!")
self.scene_objects = []
self.attached_objects = []
def clear_octomap(self):
rospy.loginfo("Clearing octomap")
self.clear_octomap_srv.call(EmptyRequest())
rospy.sleep(2.0)
def add_mesh_object(self, id_name, pose, size = (1, 1, 1), rotation = (0, 0, 0), frame='/world'):
pass
def add_box_object(self, id_name, pose, size = (1, 1, 1), rotation = (0, 0, 0), frame='/world'):
'''
Adds the particular BOX TYPE objects to the moveit planning scene
:param id_name: unique id that object should be labeled with
:param pose: pose of the object
:param size: size of the object
:param rotation: rotation offset in r, p, y
:param frame: frame in which the object pose is passed
:type id_name: string
:type pose: list of double of length 7 (x, y, z, q_x, q_y, q_z, q_w)
:type size: tuple of length 3
:type frame: string
'''
assert type(size) is tuple, 'size should be tuple'
assert len(size)==3, 'size should be of length 3'
assert not id_name in self.scene_objects, \
'Object with the same name already exists!'
self.scene_objects.append(id_name)
object_pose = PoseStamped()
object_pose.header.frame_id = frame
if type(pose) is list:
pose = list_to_pose(pose)
(r, p, y) = rotation
q_object_rotation = quaternion_from_euler(math.radians(r), math.radians(p), math.radians(y))
q_object = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]
q_object_final = quaternion_multiply(q_object, q_object_rotation)
x = pose.position.x
y = pose.position.y
z = pose.position.z
object_pose.pose = Pose(Point(x, y, z), Quaternion(*q_object_final))
#Add object description in scene
self.clear_octomap() #need to sleep before adding part!! https://answers.ros.org/question/209030/moveit-planningsceneinterface-addbox-not-showing-in-rviz/
self.planning_scene_interface.add_box(id_name, object_pose, size)
self.wait_for_planning_scene_object(id_name)
def remove_world_object(self, object_name):
'''
Removes a specified object from the MoveIt! planning scene
:param object_name: unique name for the object
:type object_name: string
'''
assert object_name is not None, 'Please pass in an object_name for the object!'
self.planning_scene_interface.remove_world_object(object_name)
self.clear_octomap()
def remove_all_objects(self):
'''
Removes all the objects in the current MoveIt! planning scene
'''
self.clear_octomap() #need to sleep before or else it gets skipped over...
self.planning_scene_interface.remove_world_object(None)
def attach_gripper_object(self, object_name, arm, gripper_planning_group):
'''
Attaches an object to the robot gripper
:param name: name of the object in the planning scene
:param arm: the Arm object
:param gripper_planning_group: the gripper planning group in MoveIt!
:type name: string
:type arm: Arm
:type gripper_planning_group: string
'''
object_name = str(object_name)
eef_link = arm.get_end_effector_link()
touch_links = arm.robot.get_link_names(group=gripper_planning_group)
self.clear_octomap()
self.planning_scene_interface.attach_box(eef_link, object_name, touch_links=touch_links)
def detach_gripper_object(self, object_name, arm, remove_from_world=False):
'''
Detaches an object earlier attached to the robot gripper
:param name: name of the object in the planning scene
:param arm: the Arm object
:param remove_from_world: if true, object also deleted from world
:type name: string
:type arm: Arm
:type remove_from_world: bool
'''
eef_link = arm.get_end_effector_link()
self.planning_scene_interface.remove_attached_object(eef_link, object_name)
if remove_from_world is True:
self.remove_world_object(object_name)
self.clear_octomap()
def wait_for_planning_scene_object(self, object_name):
'''
Waits for object to appear in planning scene
:param object_name: name of the object in the planning scene
:type object_name: string
'''
rospy.loginfo("Waiting for object '" + object_name + "'' to appear in planning scene...")
gps_req = GetPlanningSceneRequest()
gps_req.components.components = gps_req.components.WORLD_OBJECT_NAMES
part_in_scene = False
while not rospy.is_shutdown() and not part_in_scene:
# This call takes a while when rgbd sensor is set
gps_resp = self.scene_srv.call(gps_req)
# check if object_name is in the answer
for collision_obj in gps_resp.scene.world.collision_objects:
if collision_obj.id == object_name:
part_in_scene = True
break
else:
rospy.sleep(1.0)
rospy.loginfo("'" + object_name + "'' is in scene!")
# Close gripper
#------------------------------
robot.get_current_state()
print(robot.pincher_arm.get_current_pose())
print(robot.pincher_arm.get_current_rpy())
exit()
robot.pincher_arm.set_start_state(RobotState())
fechado = closeGripper()
robot.pincher_gripper.set_joint_value_target(fechado)
gplan = robot.pincher_gripper.plan()
robot.pincher_gripper.execute(gplan)
#with closed gripper, attach the box to it
rospy.sleep(1)
scene.attach_box("gripper_link", "box", p, [0.03, 0.03, 0.03])
rospy.sleep(1)
# -----------------------------
# Go to some pose
#------------------------------
robot.get_current_state()
robot.pincher_arm.set_start_state(RobotState())
target = [0.17, 0.10, 0.028]
plan = ef_pose(target, robot.pincher_arm)
if plan is not None:
robot.pincher_arm.execute(plan)
print(plan)
else:
print('No trajectory found')
class MoveItDemo:
def __init__(self):
global obj_att
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
#Initialize robot
robot = moveit_commander.RobotCommander()
# Use the planning scene object to add or remove objects
self.scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10)
# Create a publisher for displaying object frames
self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10)
### Create a publisher for visualizing direction ###
self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the MoveIt! commander for the arm
self.right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the MoveIt! commander for the gripper
self.right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Allow 5 seconds per planning attempt
self.right_arm.set_planning_time(5)
# Prepare Action Controller for gripper
self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction)
self.ac.wait_for_server()
# Give the scene a chance to catch up
rospy.sleep(2)
# Prepare Gazebo Subscriber
self.pwh = None
self.pwh_copy = None
self.idx_targ = None
self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)
### OPEN THE GRIPPER ###
self.open_gripper()
# PREPARE THE SCENE
while self.pwh is None:
rospy.sleep(0.05)
############## CLEAR THE SCENE ################
# planning_scene.world.collision_objects.remove('target')
# Remove leftover objects from a previous run
self.scene.remove_world_object('target')
self.scene.remove_world_object('table')
# self.scene.remove_world_object(obstacle1_id)
# Remove any attached objects from a previous session
self.scene.remove_attached_object(GRIPPER_FRAME, 'target')
# Run and keep in the BG the scene generator also add the ability to kill the code with ctrl^c
timerThread = threading.Thread(target=self.scene_generator)
timerThread.daemon = True
timerThread.start()
initial_pose = PoseStamped()
initial_pose.header.frame_id = 'gazebo_world'
initial_pose.pose = target_pose.pose
print "==================== Generating Transformations ==========================="
#################### PRE GRASPING POSE #########################
M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M1[0,3] = target_pose.pose.position.x
M1[1,3] = target_pose.pose.position.y
M1[2,3] = target_pose.pose.position.z
M2 = transformations.euler_matrix(0, 1.57, 0)
M2[0,3] = 0.0 # offset about x
M2[1,3] = 0.0 # about y
M2[2,3] = 0.25 # about z
T = np.dot(M1, M2)
pre_grasping = deepcopy(target_pose)
pre_grasping.pose.position.x = T[0,3]
pre_grasping.pose.position.y = T[1,3]
pre_grasping.pose.position.z = T[2,3]
quat = transformations.quaternion_from_matrix(T)
pre_grasping.pose.orientation.x = quat[0]
pre_grasping.pose.orientation.y = quat[1]
pre_grasping.pose.orientation.z = quat[2]
pre_grasping.pose.orientation.w = quat[3]
pre_grasping.header.frame_id = 'gazebo_world'
self.plan_exec(pre_grasping)
#################### GRASPING POSE #########################
M3 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M3[0,3] = target_pose.pose.position.x
M3[1,3] = target_pose.pose.position.y
M3[2,3] = target_pose.pose.position.z
M4 = transformations.euler_matrix(0, 1.57, 0)
M4[0,3] = 0.0 # offset about x
M4[1,3] = 0.0 # about y
M4[2,3] = 0.18 # about z
T2 = np.dot(M3, M4)
grasping = deepcopy(target_pose)
grasping.pose.position.x = T2[0,3]
grasping.pose.position.y = T2[1,3]
grasping.pose.position.z = T2[2,3]
quat2 = transformations.quaternion_from_matrix(T2)
grasping.pose.orientation.x = quat2[0]
grasping.pose.orientation.y = quat2[1]
grasping.pose.orientation.z = quat2[2]
grasping.pose.orientation.w = quat2[3]
grasping.header.frame_id = 'gazebo_world'
self.plan_exec(grasping)
#Close the gripper
print "========== Waiting for gazebo to catch up =========="
self.close_gripper()
#################### ATTACH OBJECT ######################
touch_links = [GRIPPER_FRAME, 'r_gripper_l_finger_tip_link','r_gripper_r_finger_tip_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link']
#print touch_links
self.scene.attach_box(GRIPPER_FRAME, target_id, target_pose, target_size, touch_links)
# counter to let the planning scene know when to remove the object
obj_att = 1
#self.scene.remove_world_object(target_id)
#################### POST-GRASP RETREAT #########################
M5 = transformations.quaternion_matrix([initial_pose.pose.orientation.x, initial_pose.pose.orientation.y, initial_pose.pose.orientation.z, initial_pose.pose.orientation.w])
M5[0,3] = initial_pose.pose.position.x
M5[1,3] = initial_pose.pose.position.y
M5[2,3] = initial_pose.pose.position.z
M6 = transformations.euler_matrix(0, 1.57, 0)
M6[0,3] = 0.0 # offset about x
M6[1,3] = 0.0 # about y
M6[2,3] = 0.3 # about z
T3 = np.dot(M5, M6)
post_grasping = deepcopy(initial_pose)
post_grasping.pose.position.x = T3[0,3]
post_grasping.pose.position.y = T3[1,3]
post_grasping.pose.position.z = T3[2,3]
quat3 = transformations.quaternion_from_matrix(T3)
post_grasping.pose.orientation.x = quat3[0]
post_grasping.pose.orientation.y = quat3[1]
post_grasping.pose.orientation.z = quat3[2]
post_grasping.pose.orientation.w = quat3[3]
post_grasping.header.frame_id = 'gazebo_world'
self.plan_exec(post_grasping)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.52
place_pose.pose.position.y = -0.48
place_pose.pose.position.z = 0.48
place_pose.pose.orientation.w = 1.0
n_attempts = 0
max_place_attempts = 2
# Generate valid place poses
places = self.make_places(place_pose)
success = False
# Repeat until we succeed or run out of attempts
while success == False and n_attempts < max_place_attempts:
for place in places:
success = self.right_arm.place(target_id, place)
if success:
break
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
rospy.sleep(0.2)
self.open_gripper()
obj_att = None
rospy.sleep(3)
## # Initialize the grasp object
## g = Grasp()
## grasps = []
## # Set the first grasp pose to the input pose
## g.grasp_pose = pre_grasping
## g.allowed_touch_objects = [target_id]
## grasps.append(deepcopy(g))
## right_arm.pick(target_id, grasps)
# #Change the frame_id for the planning to take place!
# #target_pose.header.frame_id = 'gazebo_world'
# # Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# # Exit the script
moveit_commander.os._exit(0)
##################################################################################################################
#Get pose from Gazebo
def model_state_callback(self,msg):
self.pwh = ModelStates()
self.pwh = msg
# Generate a list of possible place poses
def make_places(self, init_pose):
# Initialize the place location as a PoseStamped message
place = PoseStamped()
# Start with the input place pose
place = init_pose
# A list of x shifts (meters) to try
x_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015]
# A list of y shifts (meters) to try
y_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015]
# A list of pitch angles to try
#pitch_vals = [0, 0.005, -0.005, 0.01, -0.01, 0.02, -0.02]
pitch_vals = [0]
# A list of yaw angles to try
yaw_vals = [0]
# A list to hold the places
places = []
# Generate a place pose for each angle and translation
for y in yaw_vals:
for p in pitch_vals:
for y in y_vals:
for x in x_vals:
place.pose.position.x = init_pose.pose.position.x + x
place.pose.position.y = init_pose.pose.position.y + y
# Create a quaternion from the Euler angles
q = quaternion_from_euler(0, p, y)
# Set the place pose orientation accordingly
place.pose.orientation.x = q[0]
place.pose.orientation.y = q[1]
place.pose.orientation.z = q[2]
place.pose.orientation.w = q[3]
# Append this place pose to the list
places.append(deepcopy(place))
# Return the list
return places
def plan_exec(self, pose):
self.right_arm.clear_pose_targets()
self.right_arm.set_pose_target(pose, GRIPPER_FRAME)
self.right_arm.plan()
rospy.sleep(5)
self.right_arm.go(wait=True)
def close_gripper(self):
g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.035, 100))
self.ac.send_goal(g_close)
self.ac.wait_for_result()
rospy.sleep(15) # Gazebo requires up to 15 seconds to attach object
def open_gripper(self):
g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100))
self.ac.send_goal(g_open)
self.ac.wait_for_result()
rospy.sleep(5) # And up to 20 to detach it
def scene_generator(self):
# print obj_att
global target_pose
global target_id
global target_size
target_id = 'target'
self.taid = self.pwh.name.index('wood_cube_5cm')
table_id = 'table'
self.tid = self.pwh.name.index('table')
#obstacle1_id = 'obstacle1'
#self.o1id = self.pwh.name.index('wood_block_10_2_1cm')
# Set the target size [l, w, h]
target_size = [0.05, 0.05, 0.05]
table_size = [1.5, 0.8, 0.03]
#obstacle1_size = [0.1, 0.025, 0.01]
## Set the target pose on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose = self.pwh.pose[self.taid]
target_pose.pose.position.z += 0.025
# Add the target object to the scene
if obj_att is None:
self.scene.add_box(target_id, target_pose, target_size)
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose = self.pwh.pose[self.tid]
table_pose.pose.position.z += 1
self.scene.add_box(table_id, table_pose, table_size)
#obstacle1_pose = PoseStamped()
#obstacle1_pose.header.frame_id = REFERENCE_FRAME
#obstacle1_pose.pose = self.pwh.pose[self.o1id]
## Add the target object to the scene
#scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.50
place_pose.pose.position.y = -0.30
place_pose.pose.orientation.w = 1.0
# Add the target object to the scene
self.scene.add_box(target_id, target_pose, target_size)
### Make the target purple ###
self.setColor(target_id, 0.6, 0, 1, 1.0)
# Send the colors to the planning scene
self.sendColors()
else:
self.scene.remove_world_object('target')
# Publish targe's frame
#self.object_frames_pub.publish(target_pose)
threading.Timer(0.5, self.scene_generator).start()
# Set the color of an object
def setColor(self, name, r, g, b, a = 0.9):
# Initialize a MoveIt color object
color = ObjectColor()
# Set the id to the name given as an argument
color.id = name
# Set the rgb and alpha values given as input
color.color.r = r
color.color.g = g
color.color.b = b
color.color.a = a
# Update the global color dictionary
self.colors[name] = color
# Actually send the colors to MoveIt!
def sendColors(self):
# Initialize a planning scene object
p = PlanningScene()
# Need to publish a planning scene diff
p.is_diff = True
# Append the colors from the global color dictionary
for color in self.colors.values():
p.object_colors.append(color)
# Publish the scene diff
self.scene_pub.publish(p)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_attached_object_demo')
# 初始化场景对象
scene = PlanningSceneInterface()
rospy.sleep(1)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('manipulator')
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.05)
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 控制机械臂回到初始化位置
arm.set_named_target('home')
arm.go()
# 设置每次运动规划的时间限制：10s
arm.set_planning_time(10)
# 移除场景中之前运行残留的物体
scene.remove_attached_object(end_effector_link, 'tool')
scene.remove_world_object('table')
# 设置桌面的高度
table_ground = 0.6
# 设置table和tool的三维尺寸
table_size = [0.1, 0.3, 0.02]
tool_size = [0.2, 0.02, 0.02]
# 设置tool的位姿
p = PoseStamped()
p.header.frame_id = end_effector_link
p.pose.position.x = tool_size[0] / 2.0
p.pose.position.y = 0
p.pose.position.z = -0.015
p.pose.orientation.w = 1
# 将tool附着到机器人的终端
scene.attach_box(end_effector_link, 'tool', p, tool_size)
# 将table加入场景当中
table_pose = PoseStamped()
table_pose.header.frame_id = 'base_link'
table_pose.pose.position.x = 0.3
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box('table', table_pose, table_size)
rospy.sleep(2)
# 更新当前的位姿
arm.set_start_state_to_current_state()
# 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
joint_positions = [0.827228546495185, 0.29496592875743577, 1.1185644936946095, -0.7987583317769674, -0.18950024740190782, 0.11752152218233858]
arm.set_joint_value_target(joint_positions)
# 控制机械臂完成运动
arm.go()
rospy.sleep(1)
# 控制机械臂回到初始化位置
arm.set_named_target('home')
arm.go()
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
rospy.on_shutdown(self.shutdown)
# Construct the initial scene object
scene = PlanningSceneInterface()
rospy.sleep(1)
# Connect to the right_arm move group
right_arm = moveit_commander.MoveGroupCommander('arm')
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Display the name of the end_effector link
rospy.loginfo("The end effector link is: " + str(end_effector_link))
right_arm.allow_replanning(True)
# Set the solve method.
#right_arm.set_planner_id('RRTConnectkConfigDefault')
# Set a small tolerance on joint angles
right_arm.set_goal_joint_tolerance(0.0001)
right_arm.set_goal_position_tolerance(0.00001)
right_arm.set_goal_orientation_tolerance(0.0001)
# Set the position of the mesh file.
file = '/home/jyk/catkin_ws/src/kuca/meshes/block.STL'
# Set the planning time.
right_arm.set_planning_time(5)
total = 25
for i in range(total):
#scene.remove_attached_object('block' + str(i))
scene.remove_world_object('block' + str(i))
rospy.sleep(1)
# Start the arm target in "right" pose stored in the SRDF file
right_arm.set_named_target('right')
# Plan a trajectory to the goal configuration
traj = right_arm.plan()
# Execute the planned trajectory
right_arm.execute(traj)
# Pause for a moment
rospy.sleep(1)
###########
print right_arm.get_current_joint_values()
print '#' * 10
###########
# Make a block in fixed pose
q = quaternion_from_euler(0, 0, 1.5707)
target_pose = right_arm.get_current_pose()
target_pose.pose.position.x -= 0.8
target_pose.pose.position.y += 0.9
target_pose.pose.position.z -= 0.5
target_pose.pose.orientation.x = q[0]
target_pose.pose.orientation.y = q[1]
target_pose.pose.orientation.z = q[2]
target_pose.pose.orientation.w = q[3]
object_pose = deepcopy(target_pose)
refer_place_pose = PoseStamped()
refer_place_pose.header.frame_id = 'base_footprint'
refer_place_pose.pose.position.x = 0.8
refer_place_pose.pose.position.y = 0.1
refer_place_pose.pose.position.z = 0.6
refer_place_pose.pose.orientation.x = 0
refer_place_pose.pose.orientation.y = 0
refer_place_pose.pose.orientation.z = 0
refer_place_pose.pose.orientation.w = 1
count = 0
while (count < total):
# Set the size of block
block_size = [0.01, 0.05, 0.05]
place_pose = deepcopy(refer_place_pose)
place_pose.pose.position.y += 0.051 * (count % 5)
place_pose.pose.position.z -= 0.051 * (count // 5)
scene.add_box('block' + str(count), object_pose, block_size)
rospy.sleep(1)
#right_arm.set_joint_value_target(target_pose, end_effector_link, True)
target_pose.header.stamp = rospy.Time()
right_arm.set_pose_target(target_pose)
print target_pose
print '&' * 10
right_arm.go()
#rospy.sleep(0.5)
print right_arm.get_current_pose()
print '-' * 30
print right_arm.get_goal_tolerance()
# Create a pose for the block relative to the end-effector
p = PoseStamped()
p.header.frame_id = end_effector_link
# Place the end of the object within the grasp of the gripper
p.pose.position.x = 0
p.pose.position.y = 0.0
p.pose.position.z = 0.0
# Align the object with the gripper (straight out)
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1
# Attach the tool to the end-effector
scene.attach_box(end_effector_link, 'block' + str(count), p,
block_size)
rospy.sleep(1)
#right_arm.set_start_state_to_current_state()
place_pose.header.stamp = rospy.Time()
right_arm.set_pose_target(place_pose)
print place_pose
right_arm.go()
rospy.sleep(0.5)
print right_arm.get_current_pose().pose
print '*' * 30
right_arm.detach_object(end_effector_link)
#scene.remove_attached_object(end_effector_link, 'block'+str(count))
#scene.remove_world_object('block'+str(count))
rospy.sleep(1)
count += 1
# Return the arm to the named "resting" pose stored in the SRDF file
right_arm.set_named_target('right')
right_arm.go()
rospy.sleep(1)
# Cleanly shut down MoveIt
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
class MoveGroupCommandInterpreter(object):
"""
Interpreter for simple commands
"""
DEFAULT_FILENAME = "move_group.cfg"
GO_DIRS = {"up" : (2,1), "down" : (2, -1), "z" : (2, 1),
"left" : (1, 1), "right" : (1, -1), "y" : (1, 1),
"forward" : (0, 1), "backward" : (0, -1), "back" : (0, -1), "x" : (0, 1) }
def __init__(self):
self._gdict = {}
self._group_name = ""
self._prev_group_name = ""
self._planning_scene_interface = PlanningSceneInterface()
self._robot = RobotCommander()
self._last_plan = None
self._db_host = None
self._db_port = 33829
self._trace = False
def get_active_group(self):
if len(self._group_name) > 0:
return self._gdict[self._group_name]
else:
return None
def get_loaded_groups(self):
return self._gdict.keys()
def execute(self, cmd):
cmd = self.resolve_command_alias(cmd)
result = self.execute_generic_command(cmd)
if result != None:
return result
else:
if len(self._group_name) > 0:
return self.execute_group_command(self._gdict[self._group_name], cmd)
else:
return (MoveGroupInfoLevel.INFO, self.get_help() + "\n\nNo groups initialized yet. You must call the 'use' or the 'load' command first.\n")
def execute_generic_command(self, cmd):
if os.path.isfile("cmd/" + cmd):
cmd = "load cmd/" + cmd
cmdlow = cmd.lower()
if cmdlow.startswith("use"):
if cmdlow == "use":
return (MoveGroupInfoLevel.INFO, "\n".join(self.get_loaded_groups()))
clist = cmd.split()
clist[0] = clist[0].lower()
if len(clist) == 2:
if clist[0] == "use":
if clist[1] == "previous":
clist[1] = self._prev_group_name
if len(clist[1]) == 0:
return (MoveGroupInfoLevel.DEBUG, "OK")
if self._gdict.has_key(clist[1]):
self._prev_group_name = self._group_name
self._group_name = clist[1]
return (MoveGroupInfoLevel.DEBUG, "OK")
else:
try:
mg = MoveGroupCommander(clist[1])
self._gdict[clist[1]] = mg
self._group_name = clist[1]
return (MoveGroupInfoLevel.DEBUG, "OK")
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.FAIL, "Initializing " + clist[1] + ": " + str(e))
except:
return (MoveGroupInfoLevel.FAIL, "Unable to initialize " + clist[1])
elif cmdlow.startswith("trace"):
if cmdlow == "trace":
return (MoveGroupInfoLevel.INFO, "trace is on" if self._trace else "trace is off")
clist = cmdlow.split()
if clist[0] == "trace" and clist[1] == "on":
self._trace = True
return (MoveGroupInfoLevel.DEBUG, "OK")
if clist[0] == "trace" and clist[1] == "off":
self._trace = False
return (MoveGroupInfoLevel.DEBUG, "OK")
elif cmdlow.startswith("load"):
filename = self.DEFAULT_FILENAME
clist = cmd.split()
if len(clist) > 2:
return (MoveGroupInfoLevel.WARN, "Unable to parse load command")
if len(clist) == 2:
filename = clist[1]
if not os.path.isfile(filename):
filename = "cmd/" + filename
line_num = 0
line_content = ""
try:
f = open(filename, 'r')
for line in f:
line_num += 1
line = line.rstrip()
line_content = line
if self._trace:
print ("%s:%d: %s" % (filename, line_num, line_content))
comment = line.find("#")
if comment != -1:
line = line[0:comment].rstrip()
if line != "":
self.execute(line.rstrip())
line_content = ""
return (MoveGroupInfoLevel.DEBUG, "OK")
except MoveItCommanderException as e:
if line_num > 0:
return (MoveGroupInfoLevel.WARN, "Error at %s:%d: %s\n%s" % (filename, line_num, line_content, str(e)))
else:
return (MoveGroupInfoLevel.WARN, "Processing " + filename + ": " + str(e))
except:
if line_num > 0:
return (MoveGroupInfoLevel.WARN, "Error at %s:%d: %s" % (filename, line_num, line_content))
else:
return (MoveGroupInfoLevel.WARN, "Unable to load " + filename)
elif cmdlow.startswith("save"):
filename = self.DEFAULT_FILENAME
clist = cmd.split()
if len(clist) > 2:
return (MoveGroupInfoLevel.WARN, "Unable to parse save command")
if len(clist) == 2:
filename = clist[1]
try:
f = open(filename, 'w')
for gr in self._gdict.keys():
f.write("use " + gr + "\n")
known = self._gdict[gr].get_remembered_joint_values()
for v in known.keys():
f.write(v + " = [" + " ".join([str(x) for x in known[v]]) + "]\n")
if self._db_host != None:
f.write("database " + self._db_host + " " + str(self._db_port) + "\n")
return (MoveGroupInfoLevel.DEBUG, "OK")
except:
return (MoveGroupInfoLevel.WARN, "Unable to save " + filename)
else:
return None
def execute_group_command(self, g, cmdorig):
cmd = cmdorig.lower()
if cmd == "stop":
g.stop()
return (MoveGroupInfoLevel.DEBUG, "OK")
if cmd == "id":
return (MoveGroupInfoLevel.INFO, g.get_name())
if cmd == "help":
return (MoveGroupInfoLevel.INFO, self.get_help())
if cmd == "vars":
known = g.get_remembered_joint_values()
return (MoveGroupInfoLevel.INFO, "[" + " ".join(known.keys()) + "]")
if cmd == "joints":
joints = g.get_joints()
return (MoveGroupInfoLevel.INFO, "\n" + "\n".join([str(i) + " = " + joints[i] for i in range(len(joints))]) + "\n")
if cmd == "show":
return self.command_show(g)
if cmd == "current":
return self.command_current(g)
if cmd == "ground":
pose = PoseStamped()
pose.pose.position.x = 0
pose.pose.position.y = 0
# offset such that the box is 0.1 mm below ground (to prevent collision with the robot itself)
pose.pose.position.z = -0.0501
pose.pose.orientation.x = 0
pose.pose.orientation.y = 0
pose.pose.orientation.z = 0
pose.pose.orientation.w = 1
pose.header.stamp = rospy.get_rostime()
pose.header.frame_id = self._robot.get_root_link()
self._planning_scene_interface.attach_box(self._robot.get_root_link(), "ground", pose, (3, 3, 0.1))
return (MoveGroupInfoLevel.INFO, "Added ground")
if cmd == "eef":
if len(g.get_end_effector_link()) > 0:
return (MoveGroupInfoLevel.INFO, g.get_end_effector_link())
else:
return (MoveGroupInfoLevel.INFO, "There is no end effector defined")
if cmd == "database":
if self._db_host == None:
return (MoveGroupInfoLevel.INFO, "Not connected to a database")
else:
return (MoveGroupInfoLevel.INFO, "Connected to " + self._db_host + ":" + str(self._db_port))
if cmd == "plan":
if self._last_plan != None:
return (MoveGroupInfoLevel.INFO, str(self._last_plan))
else:
return (MoveGroupInfoLevel.INFO, "No previous plan")
if cmd == "constrain":
g.clear_path_constraints()
return (MoveGroupInfoLevel.SUCCESS, "Cleared path constraints")
if cmd == "tol" or cmd == "tolerance":
return (MoveGroupInfoLevel.INFO, "Joint = %0.6g, Position = %0.6g, Orientation = %0.6g" % g.get_goal_tolerance())
if cmd == "time":
return (MoveGroupInfoLevel.INFO, str(g.get_planning_time()))
if cmd == "execute":
if self._last_plan != None:
if g.execute(self._last_plan):
return (MoveGroupInfoLevel.SUCCESS, "Plan submitted for execution")
else:
return (MoveGroupInfoLevel.WARN, "Failed to submit plan for execution")
else:
return (MoveGroupInfoLevel.WARN, "No motion plan computed")
# see if we have assignment between variables
assign_match = re.match(r"^(\w+)\s*=\s*(\w+)$", cmd)
if assign_match:
known = g.get_remembered_joint_values()
if known.has_key(assign_match.group(2)):
g.remember_joint_values(assign_match.group(1), known[assign_match.group(2)])
return (MoveGroupInfoLevel.SUCCESS, assign_match.group(1) + " is now the same as " + assign_match.group(2))
else:
return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
# see if we have assignment of matlab-like vector syntax
set_match = re.match(r"^(\w+)\s*=\s*\[([\d\.e\-\+\s]+)\]$", cmd)
if set_match:
try:
g.remember_joint_values(set_match.group(1), [float(x) for x in set_match.group(2).split()])
return (MoveGroupInfoLevel.SUCCESS, "Remembered joint values [" + set_match.group(2) + "] under the name " + set_match.group(1))
except:
return (MoveGroupInfoLevel.WARN, "Unable to parse joint value [" + set_match.group(2) + "]")
# see if we have assignment of matlab-like element update
component_match = re.match(r"^(\w+)\s*\[\s*(\d+)\s*\]\s*=\s*([\d\.e\-\+]+)$", cmd)
if component_match:
known = g.get_remembered_joint_values()
if known.has_key(component_match.group(1)):
try:
val = known[component_match.group(1)]
val[int(component_match.group(2))] = float(component_match.group(3))
g.remember_joint_values(component_match.group(1), val)
return (MoveGroupInfoLevel.SUCCESS, "Updated " + component_match.group(1) + "[" + component_match.group(2) + "]")
except:
return (MoveGroupInfoLevel.WARN, "Unable to parse index or value in '" + cmd +"'")
else:
return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
clist = cmdorig.split()
clist[0] = clist[0].lower()
# if this is an unknown one-word command, it is probably a variable
if len(clist) == 1:
known = g.get_remembered_joint_values()
if known.has_key(cmd):
return (MoveGroupInfoLevel.INFO, "[" + " ".join([str(x) for x in known[cmd]]) + "]")
else:
return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
# command with one argument
if len(clist) == 2:
if clist[0] == "go":
self._last_plan = None
if clist[1] == "rand" or clist[1] == "random":
vals = g.get_random_joint_values()
g.set_joint_value_target(vals)
if g.go():
return (MoveGroupInfoLevel.SUCCESS, "Moved to random target [" + " ".join([str(x) for x in vals]) + "]")
else:
return (MoveGroupInfoLevel.FAIL, "Failed while moving to random target [" + " ".join([str(x) for x in vals]) + "]")
else:
try:
g.set_named_target(clist[1])
if g.go():
return (MoveGroupInfoLevel.SUCCESS, "Moved to " + clist[1])
else:
return (MoveGroupInfoLevel.FAIL, "Failed while moving to " + clist[1])
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.WARN, "Going to " + clist[1] + ": " + str(e))
except:
return (MoveGroupInfoLevel.WARN, clist[1] + " is unknown")
if clist[0] == "plan":
self._last_plan = None
vals = None
if clist[1] == "rand" or clist[1] == "random":
vals = g.get_random_joint_values()
g.set_joint_value_target(vals)
self._last_plan = g.plan()
else:
try:
g.set_named_target(clist[1])
self._last_plan = g.plan()
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.WARN, "Planning to " + clist[1] + ": " + str(e))
except:
return (MoveGroupInfoLevel.WARN, clist[1] + " is unknown")
if self._last_plan != None:
if len(self._last_plan.joint_trajectory.points) == 0 and len(self._last_plan.multi_dof_joint_trajectory.points) == 0:
self._last_plan = None
dest_str = clist[1]
if vals != None:
dest_str = "[" + " ".join([str(x) for x in vals]) + "]"
if self._last_plan != None:
return (MoveGroupInfoLevel.SUCCESS, "Planned to " + dest_str)
else:
return (MoveGroupInfoLevel.FAIL, "Failed while planning to " + dest_str)
elif clist[0] == "pick":
self._last_plan = None
if g.pick(clist[1]):
return (MoveGroupInfoLevel.SUCCESS, "Picked object " + clist[1])
else:
return (MoveGroupInfoLevel.FAIL, "Failed while trying to pick object " + clist[1])
elif clist[0] == "place":
self._last_plan = None
if g.place(clist[1]):
return (MoveGroupInfoLevel.SUCCESS, "Placed object " + clist[1])
else:
return (MoveGroupInfoLevel.FAIL, "Failed while trying to place object " + clist[1])
elif clist[0] == "planner":
g.set_planner_id(clist[1])
return (MoveGroupInfoLevel.SUCCESS, "Planner is now " + clist[1])
elif clist[0] == "record" or clist[0] == "rec":
g.remember_joint_values(clist[1])
return (MoveGroupInfoLevel.SUCCESS, "Remembered current joint values under the name " + clist[1])
elif clist[0] == "rand" or clist[0] == "random":
g.remember_joint_values(clist[1], g.get_random_joint_values())
return (MoveGroupInfoLevel.SUCCESS, "Remembered random joint values under the name " + clist[1])
elif clist[0] == "del" or clist[0] == "delete":
g.forget_joint_values(clist[1])
return (MoveGroupInfoLevel.SUCCESS, "Forgot joint values under the name " + clist[1])
elif clist[0] == "show":
known = g.get_remembered_joint_values()
if known.has_key(clist[1]):
return (MoveGroupInfoLevel.INFO, "[" + " ".join([str(x) for x in known[clist[1]]]) + "]")
else:
return (MoveGroupInfoLevel.WARN, "Joint values for " + clist[1] + " are not known")
elif clist[0] == "tol" or clist[0] == "tolerance":
try:
g.set_goal_tolerance(float(clist[1]))
return (MoveGroupInfoLevel.SUCCESS, "OK")
except:
return (MoveGroupInfoLevel.WARN, "Unable to parse tolerance value '" + clist[1] + "'")
elif clist[0] == "time":
try:
g.set_planning_time(float(clist[1]))
return (MoveGroupInfoLevel.SUCCESS, "OK")
except:
return (MoveGroupInfoLevel.WARN, "Unable to parse planning duration value '" + clist[1] + "'")
elif clist[0] == "constrain":
try:
g.set_path_constraints(clist[1])
return (MoveGroupInfoLevel.SUCCESS, "OK")
except:
if self._db_host != None:
return (MoveGroupInfoLevel.WARN, "Constraint " + clist[1] + " is not known.")
else:
return (MoveGroupInfoLevel.WARN, "Not connected to a database.")
elif clist[0] == "wait":
try:
time.sleep(float(clist[1]))
return (MoveGroupInfoLevel.SUCCESS, clist[1] + " seconds passed")
except:
return (MoveGroupInfoLevel.WARN, "Unable to wait '" + clist[1] + "' seconds")
elif clist[0] == "database":
try:
g.set_constraints_database(clist[1], self._db_port)
self._db_host = clist[1]
return (MoveGroupInfoLevel.SUCCESS, "Connected to " + self._db_host + ":" + str(self._db_port))
except:
return (MoveGroupInfoLevel.WARN, "Unable to connect to '" + clist[1] + ":" + str(self._db_port) + "'")
else:
return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
if len(clist) == 3:
if clist[0] == "go" and self.GO_DIRS.has_key(clist[1]):
self._last_plan = None
try:
offset = float(clist[2])
(axis, factor) = self.GO_DIRS[clist[1]]
return self.command_go_offset(g, offset, factor, axis, clist[1])
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.WARN, "Going " + clist[1] + ": " + str(e))
except:
return (MoveGroupInfoLevel.WARN, "Unable to parse distance '" + clist[2] + "'")
elif clist[0] == "allow" and (clist[1] == "look" or clist[1] == "looking"):
if clist[2] == "1" or clist[2] == "true" or clist[2] == "T" or clist[2] == "True":
g.allow_looking(True)
else:
g.allow_looking(False)
return (MoveGroupInfoLevel.DEBUG, "OK")
elif clist[0] == "allow" and (clist[1] == "replan" or clist[1] == "replanning"):
if clist[2] == "1" or clist[2] == "true" or clist[2] == "T" or clist[2] == "True":
g.allow_replanning(True)
else:
g.allow_replanning(False)
return (MoveGroupInfoLevel.DEBUG, "OK")
elif clist[0] == "database":
try:
g.set_constraints_database(clist[1], int(clist[2]))
self._db_host = clist[1]
self._db_port = int(clist[2])
return (MoveGroupInfoLevel.SUCCESS, "Connected to " + self._db_host + ":" + str(self._db_port))
except:
self._db_host = None
return (MoveGroupInfoLevel.WARN, "Unable to connect to '" + clist[1] + ":" + clist[2] + "'")
if len(clist) == 4:
if clist[0] == "rotate":
try:
g.set_rpy_target([float(x) for x in clist[1:]])
if g.go():
return (MoveGroupInfoLevel.SUCCESS, "Rotation complete")
else:
return (MoveGroupInfoLevel.FAIL, "Failed while rotating to " + " ".join(clist[1:]))
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.WARN, str(e))
except:
return (MoveGroupInfoLevel.WARN, "Unable to parse X-Y-Z rotation values '" + " ".join(clist[1:]) + "'")
if len(clist) >= 7:
if clist[0] == "go":
self._last_plan = None
approx = False
if (len(clist) > 7):
if (clist[7] == "approx" or clist[7] == "approximate"):
approx = True
try:
p = Pose()
p.position.x = float(clist[1])
p.position.y = float(clist[2])
p.position.z = float(clist[3])
q = tf.transformations.quaternion_from_euler(float(clist[4]), float(clist[5]), float(clist[6]))
p.orientation.x = q[0]
p.orientation.y = q[1]
p.orientation.z = q[2]
p.orientation.w = q[3]
if approx:
g.set_joint_value_target(p, True)
else:
g.set_pose_target(p)
if g.go():
return (MoveGroupInfoLevel.SUCCESS, "Moved to pose target\n%s\n" % (str(p)))
else:
return (MoveGroupInfoLevel.FAIL, "Failed while moving to pose \n%s\n" % (str(p)))
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.WARN, "Going to pose target: %s" % (str(e)))
except:
return (MoveGroupInfoLevel.WARN, "Unable to parse pose '" + " ".join(clist[1:]) + "'")
return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
def command_show(self, g):
known = g.get_remembered_joint_values()
res = []
for k in known.keys():
res.append(k + " = [" + " ".join([str(x) for x in known[k]]) + "]")
return (MoveGroupInfoLevel.INFO, "\n".join(res))
def command_current(self, g):
res = "joints = [" + " ".join([str(x) for x in g.get_current_joint_values()]) + "]"
if len(g.get_end_effector_link()) > 0:
res = res + "\n" + g.get_end_effector_link() + " pose = [\n" + str(g.get_current_pose()) + " ]"
res = res + "\n" + g.get_end_effector_link() + " RPY = " + str(g.get_current_rpy())
return (MoveGroupInfoLevel.INFO, res)
def command_go_offset(self, g, offset, factor, dimension_index, direction_name):
if g.has_end_effector_link():
try:
g.shift_pose_target(dimension_index, factor * offset)
if g.go():
return (MoveGroupInfoLevel.SUCCESS, "Moved " + direction_name + " by " + str(offset) + " m")
else:
return (MoveGroupInfoLevel.FAIL, "Failed while moving " + direction_name)
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.WARN, str(e))
except:
return (MoveGroupInfoLevel.WARN, "Unable to process pose update")
else:
return (MoveGroupInfoLevel.WARN, "No known end effector. Cannot move " + direction_name)
def resolve_command_alias(self, cmdorig):
cmd = cmdorig.lower()
if cmd == "which":
return "id"
if cmd == "groups":
return "use"
return cmdorig
def get_help(self):
res = []
res.append("Known commands:")
res.append(" help show this screen")
res.append(" id|which display the name of the group that is operated on")
res.append(" load [<file>] load a set of interpreted commands from a file")
res.append(" save [<file>] save the currently known variables as a set of commands")
res.append(" use <name> switch to using the group named <name> (and load it if necessary)")
res.append(" use|groups show the group names that are already loaded")
res.append(" vars display the names of the known states")
res.append(" show display the names and values of the known states")
res.append(" show <name> display the value of a state")
res.append(" record <name> record the current joint values under the name <name>")
res.append(" delete <name> forget the joint values under the name <name>")
res.append(" current show the current state of the active group")
res.append(" constrain <name> use the constraint <name> as a path constraint")
res.append(" constrain clear path constraints")
res.append(" eef print the name of the end effector attached to the current group")
res.append(" go <name> plan and execute a motion to the state <name>")
res.append(" go <dir> <dx>| plan and execute a motion in direction up|down|left|right|forward|backward for distance <dx>")
res.append(" go rand plan and execute a motion to a random state")
res.append(" plan <name> plan a motion to the state <name>")
res.append(" execute execute a previously computed motion plan")
res.append(" rotate <x> <y> <z> plan and execute a motion to a specified orientation (about the X,Y,Z axes)")
res.append(" tolerance show the tolerance for reaching the goal region")
res.append(" tolerance <val> set the tolerance for reaching the goal region")
res.append(" wait <dt> sleep for <dt> seconds")
res.append(" x = y assign the value of y to x")
res.append(" x[idx] = val assign a value to dimension idx of x")
res.append(" x = [v1 v2...] assign a vector of values to x")
res.append(" trace <on|off> enable/disable replanning or looking around")
res.append(" ground add a ground plane to the planning scene")
res.append(" allow replanning <true|false> enable/disable replanning")
res.append(" allow looking <true|false> enable/disable looking around")
return "\n".join(res)
def get_keywords(self):
known_vars = []
known_constr = []
if self.get_active_group() != None:
known_vars = self.get_active_group().get_remembered_joint_values().keys()
known_constr = self.get_active_group().get_known_constraints()
groups = self._robot.get_group_names()
return {'go':['up', 'down', 'left', 'right', 'backward', 'forward', 'random'] + known_vars,
'help':[],
'record':known_vars,
'show':known_vars,
'wait':[],
'delete':known_vars,
'database': [],
'current':[],
'use':groups,
'load':[],
'save':[],
'pick':[],
'place':[],
'plan':known_vars,
'allow':['replanning', 'looking'],
'constrain':known_constr,
'vars':[],
'joints':[],
'tolerance':[],
'time':[],
'eef':[],
'execute':[],
'ground':[],
'id':[]}
tool_size = [0.2, 0.02, 0.02]
#############################################
# 设置tool的位姿
p = PoseStamped()
p.header.frame_id = end_effector_link
p.pose.position.x = tool_size[0] / 2.0 - 0.025
p.pose.position.y = -0.015
p.pose.position.z = 0.0
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1
scene.attach_box(end_effector_link, 'tool', p, tool_size)
# ############################################
# 将obstacle加入场景当中
obstacle_pose = PoseStamped()
obstacle_pose.header.frame_id = 'base_link'
obstacle_pose.pose.position.x = 0.35
obstacle_pose.pose.position.y = 0.0
obstacle_pose.pose.position.z = 0.33
obstacle_pose.pose.orientation.w = 1.0
scene.add_box('obstacle', obstacle_pose, obstacle_size)
rospy.sleep(2)
############################################
target_pose = PoseStamped()
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
# Construct the initial scene object
scene = PlanningSceneInterface()
rospy.sleep(1)
# Connect to the right_arm move group
right_arm = moveit_commander.MoveGroupCommander('arm')
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Display the name of the end_effector link
rospy.loginfo("The end effector link is: " + str(end_effector_link))
# Set a small tolerance on joint angles
right_arm.set_goal_joint_tolerance(0.001)
right_arm.set_goal_position_tolerance(0.0001)
scene.remove_world_object('block')
# Start the arm target in "contract" pose stored in the SRDF file
# right_arm.set_named_target('contract')
# right_arm.go()
# rospy.sleep(2)
# Start the arm target in "right" pose stored in the SRDF file
right_arm.set_named_target('right')
# Plan a trajectory to the goal configuration
traj = right_arm.plan()
# Execute the planned trajectory
right_arm.execute(traj)
# Pause for a moment
rospy.sleep(1)
start_pose = right_arm.get_current_pose().pose
waypoints = []
wpose = deepcopy(start_pose)
waypoints.append(deepcopy(wpose))
wpose.position.x -= 0.2
waypoints.append(deepcopy(wpose))
wpose.position.y += 0.3
waypoints.append(deepcopy(wpose))
wpose.position.z -= 0.5
waypoints.append(deepcopy(wpose))
fraction = 0.0
maxtries = 100
attempts = 0
# Set the internal state to the current state
right_arm.set_start_state_to_current_state()
# Plan the Cartesian path connecting the waypoints
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = right_arm.compute_cartesian_path(
waypoints, # waypoint poses
0.01, # eef_step
0.0, # jump_threshold
True) # avoid_collisions
# Increment the number of attempts
attempts += 1
# Print out a progress message
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
# If we have a complete plan, execute the trajectory
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
right_arm.execute(plan)
rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed with only " + str(fraction) +
" success after " + str(maxtries) + " attempts.")
# Set the size of block
block_size = [0.01, 0.05, 0.05]
# Create a pose for the block relative to the end-effector
p = PoseStamped()
p.header.frame_id = end_effector_link
# Place the end of the object within the grasp of the gripper
p.pose.position.x = -0.015
p.pose.position.y = 0.0
p.pose.position.z = 0.0
# Align the object with the gripper (straight out)
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1
# Attach the tool to the end-effector
scene.attach_box(end_effector_link, 'block', p, block_size)
waypoints = []
start_pose = right_arm.get_current_pose().pose
wpose = deepcopy(start_pose)
waypoints.append(deepcopy(wpose))
wpose.position.z += 0.25
waypoints.append(deepcopy(wpose))
#wpose.position.y -= 0.30
#waypoints.append(deepcopy(wpose))
fraction = 0.0
maxtries = 100
attempts = 0
# Set the internal state to the current state
right_arm.set_start_state_to_current_state()
# Plan the Cartesian path connecting the waypoints
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = right_arm.compute_cartesian_path(
waypoints, # waypoint poses
0.01, # eef_step
0.0, # jump_threshold
True) # avoid_collisions
# Increment the number of attempts
attempts += 1
# Print out a progress message
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
# If we have a complete plan, execute the trajectory
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
right_arm.execute(plan)
rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed with only " + str(fraction) +
" success after " + str(maxtries) + " attempts.")
joint_positions = right_arm.get_current_joint_values()
joint_positions[0] = -1.5707
right_arm.set_joint_value_target(joint_positions)
right_arm.go()
rospy.sleep(1)
waypoints = []
start_pose = right_arm.get_current_pose().pose
wpose = deepcopy(start_pose)
waypoints.append(deepcopy(wpose))
wpose.position.z -= 0.25
waypoints.append(deepcopy(wpose))
fraction = 0.0
maxtries = 100
attempts = 0
# Set the internal state to the current state
right_arm.set_start_state_to_current_state()
# Plan the Cartesian path connecting the waypoints
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = right_arm.compute_cartesian_path(
waypoints, # waypoint poses
0.01, # eef_step
0.0, # jump_threshold
True) # avoid_collisions
# Increment the number of attempts
attempts += 1
# Print out a progress message
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
# If we have a complete plan, execute the trajectory
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
right_arm.execute(plan)
rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed with only " + str(fraction) +
" success after " + str(maxtries) + " attempts.")
scene.remove_attached_object(end_effector_link, 'block')
rospy.sleep(1)
waypoints = []
start_pose = right_arm.get_current_pose().pose
wpose = deepcopy(start_pose)
waypoints.append(deepcopy(wpose))
wpose.position.z += 0.15
waypoints.append(deepcopy(wpose))
#wpose.position.y -= 0.30
#waypoints.append(deepcopy(wpose))
fraction = 0.0
maxtries = 100
attempts = 0
# Set the internal state to the current state
right_arm.set_start_state_to_current_state()
# Plan the Cartesian path connecting the waypoints
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = right_arm.compute_cartesian_path(
waypoints, # waypoint poses
0.01, # eef_step
0.0, # jump_threshold
True) # avoid_collisions
# Increment the number of attempts
attempts += 1
# Print out a progress message
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
# If we have a complete plan, execute the trajectory
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
right_arm.execute(plan)
rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed with only " + str(fraction) +
" success after " + str(maxtries) + " attempts.")
rospy.sleep(1)
joint_positions = right_arm.get_current_joint_values()
joint_positions[0] = 0
joint_positions[4] = 0
joint_positions[5] = 0
right_arm.set_joint_value_target(joint_positions)
right_arm.go()
rospy.sleep(1)
# Return the arm to the named "resting" pose stored in the SRDF file
right_arm.set_named_target('right')
right_arm.go()
rospy.sleep(1)
# Cleanly shut down MoveIt
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
class Pick(object):
_feedback = moveit_msgs.msg.PickupActionFeedback().feedback
_result = moveit_msgs.msg.PickupActionResult().result
def __init__(self, name):
self._action_name = name
self._as = actionlib.SimpleActionServer(self._action_name,
moveit_msgs.msg.PickupAction,
execute_cb=self.execute_cb,
auto_start=False)
self._as.start()
rospy.loginfo('Action Service Loaded')
self.robot = Phantomx_Pincher()
rospy.loginfo('Moveit Robot Commander Loaded')
self.scene = PlanningSceneInterface()
rospy.loginfo('Moveit Planning Scene Loaded')
rospy.loginfo('Pick action is ok. Awaiting for connections')
def get_target(self, target_name):
obj = self.scene.get_objects([target_name])
obj = obj[target_name]
pose = obj.primitive_poses[0].position
target = [pose.x, pose.y, pose.z]
size = obj.primitives[0].dimensions
rospy.loginfo(target)
return size, target
def execute_cb(self, goal):
r = rospy.Rate(1)
sucess = True
#self._result.trajectory_start = self.robot.robot.get_current_state()
self.robot.set_start_state_to_current_state()
self._feedback.state = "Open Gripper"
self._as.publish_feedback(self._feedback)
self.robot.robot.get_current_state()
rospy.loginfo('Open Gripper Plan')
plan = self.robot.openGripper()
if plan is None:
rospy.loginfo("Open Gripper plan failed")
self._result.error_code.val = -1
self._as.set_aborted(self._result)
sucess = False
return None
self._result.trajectory_descriptions.append('OpenGripper')
self._result.trajectory_stages.append(plan)
self._feedback.state = "Opening gripper"
rospy.loginfo('Opening gripper')
ex_status = self.robot.gripper_execute(plan)
if not ex_status:
rospy.loginfo("Execution to open gripper failed: [%s]", ex_status)
self._result.error_code.val = -4
self._as.set_aborted(self._result)
sucess = False
return None
self._as.publish_feedback(self._feedback)
self.scene.remove_attached_object('gripper_link')
self._feedback.state = "Planning to reach object"
rospy.loginfo('Planning to reach obj')
self._as.publish_feedback(self._feedback)
dimension, target = self.get_target(goal.target_name)
quat = []
if len(goal.possible_grasps) > 0:
quat = [
goal.possible_grasps[0].grasp_pose.pose.orientation.x,
goal.possible_grasps[0].grasp_pose.pose.orientation.y,
goal.possible_grasps[0].grasp_pose.pose.orientation.z,
goal.possible_grasps[0].grasp_pose.pose.orientation.w
]
rospy.loginfo('Pick Quaternion [%s]', quat)
plan = self.robot.ef_pose(target, dimension, orientation=quat)
if plan is None:
rospy.loginfo("Plan to grasp failed")
self._result.error_code.val = -1
self._as.set_aborted(self._result)
sucess = False
return None
self._feedback.state = "Going to the object"
rospy.loginfo('Going to Obj')
self._as.publish_feedback(self._feedback)
self._result.trajectory_descriptions.append(
"Going to grasp the object")
self._result.trajectory_stages.append(plan)
ex_status = self.robot.arm_execute(plan)
if not ex_status:
rospy.loginfo("Execution to grasp failed: [%s]", ex_status)
self._result.error_code.val = -4
self._as.set_aborted(self._result)
sucess = False
return None
# self._feedback.state = "Removing obtect to be grasp from the planning scene"
# self._as.publish_feedback(self._feedback)
obj = self.scene.get_objects([goal.target_name])
obj = obj[goal.target_name]
# self.scene.remove_world_object(goal.target_name)#
self._feedback.state = "Attaching object"
self._as.publish_feedback(self._feedback)
pose = PoseStamped()
pose.pose = obj.primitive_poses[0]
pose.header = obj.header
self.scene.attach_box(
"gripper_link", obj.id, pose, obj.primitives[0].dimensions,
['gripper_link', 'gripper_active_link', 'gripper_active2_link'])
rospy.sleep(1)
rospy.sleep(1)
self._feedback.state = "Planning to close the gripper"
self._as.publish_feedback(self._feedback)
plan = self.robot.closeGripper()
if plan is None:
rospy.loginfo("Close Gripper plan failed")
self._result.error_code.val = -1
self._as.set_aborted(self._result)
sucess = False
return None
self._result.trajectory_descriptions.append('CloseGripper')
self._result.trajectory_stages.append(plan)
self._feedback.state = "Closing gripper"
ex_status = self.robot.gripper_execute(plan)
if not ex_status:
rospy.loginfo("Execution to grasp failed: [%s]", ex_status)
self._result.error_code.val = -4
self._as.set_aborted(self._result)
sucess = False
return None
self._as.publish_feedback(self._feedback)
if sucess:
self._result.error_code.val = 1
self._as.set_succeeded(self._result)
class PickAndPlace:
def setColor(self, name, r, g, b, a=0.9):
color = ObjectColor()
color.id = name
color.color.r = r
color.color.g = g
color.color.b = b
color.color.a = a
self.colors[name] = color
def sendColors(self):
p = PlanningScene()
p.is_diff = True
for color in self.colors.values():
p.object_colors.append(color)
self.scene_pub.publish(p)
def add_point(self, traj, time, positions, velocities=None):
point = trajectory_msgs.msg.JointTrajectoryPoint()
point.positions = copy.deepcopy(positions)
if velocities is not None:
point.velocities = copy.deepcopy(velocities)
point.time_from_start = rospy.Duration(time)
traj.points.append(point)
def FollowQTraj(self, q_traj, t_traj):
assert (len(q_traj) == len(t_traj))
#Insert current position to beginning.
if t_traj[0] > 1.0e-2:
t_traj.insert(0, 0.0)
q_traj.insert(0, self.Q(arm=arm))
self.dq_traj = self.QTrajToDQTraj(q_traj, t_traj)
#self.traj= self.ToROSTrajectory(self.JointNames(), q_traj, t_traj, dq_traj) #, dq_traj
#print traj
#self.sub_jpc.publish(self.ToROSTrajectory(self.JointNames(), q_traj, t_traj, dq_traj))
def QTrajToDQTraj(self, q_traj, t_traj):
dof = len(q_traj[0])
#Modeling the trajectory with spline.
splines = [TCubicHermiteSpline() for d in range(dof)]
for d in range(len(splines)):
data_d = [[t, q[d]] for q, t in zip(q_traj, t_traj)]
splines[d].Initialize(data_d,
tan_method=splines[d].CARDINAL,
c=0.0,
m=0.0)
#NOTE: We don't have to make spline models as we just want velocities at key points.
# They can be obtained by computing tan_method, which will be more efficient. with_tan=True
dq_traj = []
for t in t_traj:
dq = [splines[d].Evaluate(t, with_tan=True)[1] for d in range(dof)]
dq_traj.append(dq)
#print dq_traj
return dq_traj
def JointNames(self):
#0arm= 0
return self.joint_names[0]
def ROSGetJTP(self, q, t, dq=None):
jp = trajectory_msgs.msg.JointTrajectoryPoint()
jp.positions = q
jp.time_from_start = rospy.Duration(t)
if dq is not None: jp.velocities = dq
return jp
def ToROSTrajectory(self, joint_names, q_traj, t_traj, dq_traj=None):
assert (len(q_traj) == len(t_traj))
if dq_traj is not None: (len(dq_traj) == len(t_traj))
#traj= trajectory_msgs.msg.JointTrajectory()
self.traj.joint_names = joint_names
if dq_traj is not None:
self.traj.points = [
self.ROSGetJTP(q, t, dq)
for q, t, dq in zip(q_traj, t_traj, dq_traj)
]
else:
self.traj.points = [
self.ROSGetJTP(q, t) for q, t in zip(q_traj, t_traj)
]
self.traj.header.stamp = rospy.Time.now()
#print self.traj
return self.traj
def SmoothQTraj(self, q_traj):
if len(q_traj) == 0: return
q_prev = np.array(q_traj[0])
q_offset = np.array([0] * len(q_prev))
for q in q_traj:
q_diff = np.array(q) - q_prev
for d in range(len(q_prev)):
if q_diff[d] < -math.pi: q_offset[d] += 1
elif q_diff[d] > math.pi: q_offset[d] -= 1
q_prev = copy.deepcopy(q)
q[:] = q + q_offset * 2.0 * math.pi
def add_target(self, f_target_pose, frame, x, y, o1, o2, o3, o4):
f_target_pose.header.frame_id = frame
f_target_pose.pose.position.x = x
f_target_pose.pose.position.y = y
f_target_pose.pose.position.z = self.table_ground + self.table_size[
2] + self.f_target_size[2] / 2.0
f_target_pose.pose.orientation.x = o1
f_target_pose.pose.orientation.y = o2
f_target_pose.pose.orientation.z = o3
f_target_pose.pose.orientation.w = o4
#self.scene.add_box(f_target_id,f_target_pose,f_target_size)
def cts(self, start_pose, end_pose, maxtries, exe_signal=False):
waypoints = []
fraction = 0.0
attempts = 0
#maxtries_z=300
waypoints.append(start_pose)
waypoints.append(end_pose)
while fraction != 1 and attempts < maxtries:
(plan, fraction) = self.arm.compute_cartesian_path(
waypoints, 0.005, 0.0, True)
attempts += 1
if (attempts % maxtries == 0 and fraction != 1):
rospy.loginfo("path planning failed with " +
str(fraction * 100) + "% success.")
signal = False
elif fraction == 1:
rospy.loginfo("path compute successfully with " +
str(attempts) + " attempts.")
if exe_signal: self.arm.execute(plan)
end_joint_state = plan.joint_trajectory.points[-1].positions
signal = True
return plan, end_joint_state, signal
# shaking function:
# freq : shaking freqence
# times : shaking time per action
def shaking(self, initial_state, start_joint_state, end_joint_state, freq,
times):
q_traj = [initial_state]
t_traj = [0.0]
for i in range(times):
q_traj.append(end_joint_state)
t_traj.append(t_traj[-1] + 0.5 / freq)
q_traj.append(start_joint_state)
t_traj.append(t_traj[-1] + 0.5 / freq)
q_traj.append(initial_state)
t_traj.append(t_traj[-1] + 0.5 / freq)
self.FollowQTraj(q_traj, t_traj)
self.sub_jpc.publish(
self.ToROSTrajectory(self.JointNames(), q_traj, t_traj,
self.dq_traj))
rospy.sleep(6)
def setupSence(self):
r_tool_size = [0.03, 0.02, 0.18]
l_tool_size = [0.03, 0.02, 0.18]
#real scene table
table_pose = PoseStamped()
table_pose.header.frame_id = self.reference_frame
table_pose.pose.position.x = -0.184
table_pose.pose.position.y = 0.62
table_pose.pose.position.z = self.table_ground + self.table_size[
2] / 2.0
table_pose.pose.orientation.w = 1.0
self.scene.add_box(self.table_id, table_pose, self.table_size)
#left gripper
l_p = PoseStamped()
l_p.header.frame_id = self.arm_end_effector_link
l_p.pose.position.x = 0.00
l_p.pose.position.y = 0.057
l_p.pose.position.z = 0.09
l_p.pose.orientation.w = 1
self.scene.attach_box(self.arm_end_effector_link, self.l_id, l_p,
l_tool_size)
#right gripper
r_p = PoseStamped()
r_p.header.frame_id = self.arm_end_effector_link
r_p.pose.position.x = 0.00
r_p.pose.position.y = -0.057
r_p.pose.position.z = 0.09
r_p.pose.orientation.w = 1
self.scene.attach_box(self.arm_end_effector_link, self.r_id, r_p,
r_tool_size)
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
self.scene = PlanningSceneInterface()
pub_traj = rospy.Publisher('/joint_path_command',
trajectory_msgs.msg.JointTrajectory,
queue_size=10)
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
self.gripperCtrl = rospy.ServiceProxy(
"/two_finger/gripper/gotoPositionUntilTouch", SetPosition)
#self.m2j=rospy.Publisher("/two_finger/motoman_control/move_to_joint",JointAnglesDuration,queue_size=1,latch=True)
self.colors = dict()
rospy.sleep(1)
self.arm = MoveGroupCommander('arm')
cartesian = rospy.get_param('~cartesian', True)
self.arm_end_effector_link = self.arm.get_end_effector_link()
self.arm.set_goal_position_tolerance(0.005)
self.arm.set_goal_orientation_tolerance(0.025)
self.arm.allow_replanning(True)
self.reference_frame = 'base_link'
self.arm.set_pose_reference_frame(self.reference_frame)
self.arm.set_planning_time(5)
#shaking
self.joint_names = [[]]
self.joint_names[0] = rospy.get_param('controller_joint_names')
self.traj = trajectory_msgs.msg.JointTrajectory()
self.sub_jpc = rospy.Publisher('/joint_path_command',
trajectory_msgs.msg.JointTrajectory,
queue_size=10)
#scene planning
self.l_id = 'l_tool'
self.r_id = 'r_tool'
self.table_id = 'table'
self.target1_id = 'target1_object'
self.target2_id = 'target2_object'
self.target3_id = 'target3_object'
self.target4_id = 'target4_object'
self.f_target_id = 'receive_container'
self.scene.remove_world_object(self.l_id)
self.scene.remove_world_object(self.r_id)
self.scene.remove_world_object(self.table_id)
self.scene.remove_world_object(self.target1_id)
self.scene.remove_world_object(self.target2_id)
self.scene.remove_world_object(self.target3_id)
self.scene.remove_world_object(self.target4_id)
#self.scene.remove_attached_object(self.arm_end_effector_link,self.target_id)
self.scene.remove_world_object(self.f_target_id)
self.table_ground = 0.13
self.table_size = [0.9, 0.6, 0.018]
self.setupSence()
joint_names = [
'joint_' + jkey for jkey in ('s', 'l', 'e', 'u', 'r', 'b', 't')
]
joint_names = rospy.get_param('controller_joint_names')
traj = trajectory_msgs.msg.JointTrajectory()
traj.joint_names = joint_names
target1_size = [0.058, 0.058, 0.19]
target2_size = [0.058, 0.058, 0.19]
target3_size = [0.058, 0.058, 0.19]
target4_size = [0.058, 0.058, 0.19]
self.f_target_size = [0.2, 0.2, 0.04]
f_target_pose = PoseStamped()
pre_pour_pose = PoseStamped()
target1_pose = PoseStamped()
target2_pose = PoseStamped()
target3_pose = PoseStamped()
target4_pose = PoseStamped()
#self.target_pose=PoseStamped()
#final target
#self.add_target(f_target_pose,self.reference_frame,-0.184+0.27,0.62+0.1,0,0,0,1)
#self.scene.add_box(self.f_target_id,f_target_pose,f_target_size)
#self.add_target(pre_pour_pose,self.reference_frame,x,y,0,0,0,1)
#pouring pose
pour_pose = f_target_pose
pour_pose.pose.position.x -= 0.06
pour_pose.pose.position.y -= 0.12
pour_pose.pose.position.z += 0.15
pour_pose.pose.orientation.x = -0.5
pour_pose.pose.orientation.y = -0.5
pour_pose.pose.orientation.z = -0.5
pour_pose.pose.orientation.w = 0.5
#grasp_pose
g_p = PoseStamped()
g_p.header.frame_id = self.arm_end_effector_link
g_p.pose.position.x = 0.00
g_p.pose.position.y = -0.00
g_p.pose.position.z = 0.025
g_p.pose.orientation.w = 0.707
g_p.pose.orientation.x = 0
g_p.pose.orientation.y = -0.707
g_p.pose.orientation.z = 0
#set color
self.setColor(self.target1_id, 0.8, 0, 0, 1.0)
self.setColor(self.target2_id, 0.8, 0, 0, 1.0)
self.setColor(self.target3_id, 0.8, 0, 0, 1.0)
self.setColor(self.target4_id, 0.8, 0, 0, 1.0)
self.setColor(self.f_target_id, 0.8, 0.4, 0, 1.0)
self.setColor('r_tool', 0.8, 0, 0)
self.setColor('l_tool', 0.8, 0, 0)
self.setColor(self.table_id, 0, 1, 0)
self.sendColors()
self.gripperCtrl(255)
rospy.sleep(3)
self.arm.set_named_target("initial_arm")
self.arm.go()
rospy.sleep(5)
j_ori_state = [
-1.899937629699707, -0.5684762597084045, 0.46537330746650696,
2.3229329586029053, -0.057941947132349014, -1.2867668867111206,
0.2628822326660156
]
signal = True
self.arm.set_joint_value_target(j_ori_state)
self.arm.go()
rospy.sleep(3)
#target localization
tar_num = 2
maxtries = 300
#msg = rospy.wait_for_message('/aruco_single/pose',PoseStamped)
self.add_target(target1_pose, self.reference_frame, -0.05, 0.6, 0, 0,
0, 1)
self.scene.add_box(self.target1_id, target1_pose, target1_size)
self.add_target(target2_pose, self.reference_frame, -0.15, 0.55, 0, 0,
0, 1)
self.scene.add_box(self.target2_id, target2_pose, target2_size)
#self.add_target(target3_pose,self.reference_frame,-0.25y,0,0,0,1)
#self.scene.add_box(target3_id,target3_pose,target3_size)
#self.add_target(target4_pose,self.reference_frame,x,y,0,0,0,1)
#self.scene.add_box(target4_id,target4_pose,target4_size)
#path planning
#input : target pose
#output : policy and execute
#cts : output : plan, end_joint_state, signal
#global target_pose,target_id,target_size
for i in range(tar_num):
start_pose = self.arm.get_current_pose(
self.arm_end_effector_link).pose
if i == 0:
target_pose = target1_pose
target_id = self.target1_id
target_size = target1_size
elif i == 1:
target_pose = target2_pose
target_id = self.target2_id
target_size = target2_size
elif i == 2:
self.target_pose = target3_pose
self.target_id = target3_id
self.target_size = target3_size
elif i == 3:
self.target_pose = target4_pose
self.target_id = target4_id
self.target_size = target4_size
#pick up
if cartesian:
plan_1, end_joint_state_1, signal_1 = self.cts(
start_pose, target_pose.pose, maxtries)
if signal_1:
self.arm.set_joint_value_target(end_joint_state_1)
self.arm.go()
rospy.sleep(5)
self.gripperCtrl(0)
rospy.sleep(2)
self.scene.attach_box(self.arm_end_effector_link,
target_id, g_p, target_size)
self.arm.set_joint_value_target(start_pose)
self.arm.go()
rospy.sleep(5)
self.arm.set_joint_value_target(end_joint_state_1)
self.arm.go()
rospy.sleep(5)
self.gripperCtrl(255)
self.scene.remove_attached_object(
self.arm_end_effector_link, target_id)
rospy.sleep(3)
self.arm.set_joint_value_target(start_pose)
self.arm.go()
rospy.sleep(5)
"""
#move to pouring pose
if cartesian:
plan_2, end_joint_state_2,signal_2=self.cts(start_pose,target_pose,maxtries)
if signal_2:
self.arm.set_joint_value_target(end_joint_state_2)
self.arm.go()
rospy.sleep(5)
#scene.remove_attached_object(end_effector_link,target_id)
#shaking
amp=
freq=
r_angle=
start_pick_pose=self.arm.get_current_pose(self.arm_end_effector_link).pose
start_joint_state=self.arm.get_current_joint_values()
shift_x_pose=deepcopy(start_pick_pose)
shift_z_pose=deepcopy(start_pick_pose)
shift_x_pose.position.x+=0.04
shift_z_pose.position.z-=0.04
#exe_sig=False
if cartesian:
plan_z,end_joint_state_z,signal_z=self.cts(start_pick_pose,shift_z_pose,300)
plan_x,end_joint_state_x,signal_x=self.cts(start_pick_pose,shift_x_pose,300)
#shaking process
#signal_x=False
shake_times=0
#self.freq=3.0
signal=True
while signal:
if (shake_times%3!=0 and signal_x) or (shake_times%3==0 and not signal_z):
self.shaking(start_joint_state,end_joint_state_x,3,3)
elif shake_times%3==0 and signal_z:
self.shaking(start_joint_state,end_joint_state_z,3,1)
shake_times+=1
rospy.loginfo("shaking times : "+str(shake_times)+ " .")
if shake_times==10: signal=False
"""
#remove and shut down
#self.scene.remove_attached_object(self.arm_end_effector_link,'l_tool')
#self.scene.remove_attached_object(self.arm_end_effector_link,'r_tool')
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_ik_demo')
# 初始化场景对象
scene = PlanningSceneInterface()
rospy.sleep(1)
# 初始化需要使用move group控制的机械臂中的arm group
arm = moveit_commander.MoveGroupCommander('manipulator')
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 设置目标位置所使用的参考坐标系
reference_frame = 'wall'
arm.set_pose_reference_frame(reference_frame)
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
arm.set_planning_time(10)
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.002)
arm.set_goal_orientation_tolerance(0.01)
# ===========移除场景中之前运行残留的物体================
scene.remove_attached_object(end_effector_link, 'tool')
scene.remove_world_object('wall')
scene.remove_world_object('floor')
scene.remove_world_object('target')
#===================在末端添加物体，防止碰撞=============
# 设置tool的三维尺寸
tool_size = [0.01, 0.12, 0.07]
# 设置tool的位姿
p = PoseStamped()
p.header.frame_id = end_effector_link
p.pose.position.x = -0.05
p.pose.position.y = 0.079
p.pose.position.z = 0.0
p.pose.orientation.x = -0.5
p.pose.orientation.y = -0.5
p.pose.orientation.z = 0.5
p.pose.orientation.w = 0.5
# 将tool附着到机器人的终端
scene.attach_box(end_effector_link, 'tool', p, tool_size)
#=======================================================
#===================在场景中添加物体，防止碰撞====================
# 设置三维尺寸
wall_size = [0.001, 2, 2]
floor_size = [2, 2, 0.001]
# 将wall加入场景当中
wall_pose = PoseStamped()
wall_pose.header.frame_id = 'wall'
wall_pose.pose.position.x = 0.0
wall_pose.pose.position.y = 0.0
wall_pose.pose.position.z = 0.0
wall_pose.pose.orientation.w = 1.0
scene.add_box('wall', wall_pose, wall_size)
rospy.sleep(2)
#=========================================================
# 设置机械臂工作空间中的目标位姿，位置使用x、y、z坐标描述，
# 姿态使用四元数描述，基于base_link坐标系
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.header.stamp = rospy.Time.now()
target_pose.pose = ccd_to_ur5_ik(-0.1,0,0,0)
print target_pose.pose
# 设置机器臂当前的状态作为运动初始状态
arm.set_start_state_to_current_state()
# 设置机械臂终端运动的目标位姿
arm.set_pose_target(target_pose, end_effector_link)
# 规划运动路径
traj = arm.plan()
# 按照规划的运动路径控制机械臂运动
arm.execute(traj)
rospy.sleep(1)
#=====================================================
#=====================================================
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
class Pick(object):
_feedback = moveit_msgs.msg.PickupActionFeedback().feedback
_result = moveit_msgs.msg.PickupActionResult().result
def __init__(self, name):
self._action_name = name
self._as = actionlib.SimpleActionServer(self._action_name,
moveit_msgs.msg.PickupAction,
execute_cb=self.execute_cb,
auto_start=False)
self._as.start()
rospy.loginfo('Action Service Loaded')
self.robot = Phantomx_Pincher()
rospy.loginfo('Moveit Robot Commander Loaded')
self.scene = PlanningSceneInterface()
rospy.loginfo('Moveit Planning Scene Loaded')
rospy.loginfo('Pick action is ok. Awaiting for connections')
def get_target(self, target_name):
obj = self.scene.get_objects([target_name])
obj = obj[target_name]
pose = obj.primitive_poses[0].position
target = [0.17, 0.005, 0.019]
rospy.loginfo(target)
target = [pose.x, pose.y, pose.z]
rospy.loginfo(target)
return target
def execute_cb(self, goal):
r = rospy.Rate(1)
sucess = True
#self._result.trajectory_start = self.robot.robot.get_current_state()
self.robot.set_start_state_to_current_state()
self._feedback.state = "Open Gripper"
self._as.publish_feedback(self._feedback)
self.robot.robot.get_current_state()
plan = self.robot.openGripper()
if plan is None:
rospy.loginfo("Open Gripper plan failed")
self._as.set_preempted()
self._result.error_code.val = -1
sucess = False
return None
self._result.trajectory_descriptions.append('OpenGripper')
self._result.trajectory_stages.append(plan)
self._feedback.state = "Opening gripper"
print self._feedback
self.robot.gripper_execute(plan)
self._as.publish_feedback(self._feedback)
rospy.sleep(1)
self._feedback.state = "Planning to reach object"
self._as.publish_feedback(self._feedback)
target = self.get_target(goal.target_name)
plan = self.robot.ef_pose(target)
if plan is None:
rospy.loginfo("Plan to grasp failed")
self._as.set_preempted()
self._result.error_code.val = -1
sucess = False
return None
self._feedback.state = "Going to the object"
self._as.publish_feedback(self._feedback)
self._result.trajectory_descriptions.append(
"Going to grasp the object")
self._result.trajectory_stages.append(plan)
self.robot.arm_execute(plan)
rospy.sleep(7)
self._feedback.state = "Removing obtect to be grasp from the planning scene"
self._as.publish_feedback(self._feedback)
obj = self.scene.get_objects([goal.target_name])
obj = obj[goal.target_name]
self.scene.remove_world_object(goal.target_name)
rospy.sleep(1)
self._feedback.state = "Planning to close the gripper"
self._as.publish_feedback(self._feedback)
plan = self.robot.closeGripper()
if plan is None:
rospy.loginfo("Close Gripper plan failed")
self._as.set_preempted()
self._result.error_code.val = -1
sucess = False
return None
self._result.trajectory_descriptions.append('CloseGripper')
self._result.trajectory_stages.append(plan)
self._feedback.state = "Closing gripper"
print self._feedback
self.robot.gripper_execute(plan)
rospy.sleep(1)
self._as.publish_feedback(self._feedback)
self._feedback.state = "Attaching object"
pose = PoseStamped()
pose.pose = obj.primitive_poses[0]
pose.header = obj.header
self.scene.attach_box(
"gripper_link", obj.id, pose, obj.primitives[0].dimensions,
['gripper_link', 'gripper_active_link', 'gripper_active2_link'])
self._as.publish_feedback(self._feedback)
if sucess:
self._result.error_code.val = 1
rospy.loginfo(self._result)
self._as.set_succeeded(self._result)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Construct the initial scene object
scene = PlanningSceneInterface()
# Pause for the scene to get ready
rospy.sleep(1)
# Initialize the MoveIt! commander for the left arm
left_arm = MoveGroupCommander('left_arm')
# Initialize the MoveIt! commander for the gripper
left_gripper = MoveGroupCommander('left_gripper')
# Get the name of the end-effector link
end_eef = left_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
left_arm.set_goal_position_tolerance(0.01)
left_arm.set_goal_orientation_tolerance(0.05)
# Allow replanning to increase the odds of a solution
left_arm.allow_replanning(True)
# Allow 5 seconds per planning attempt
left_arm.set_planning_time(5)
# Remove leftover objects from a previous run
scene.remove_attached_object(end_eef, 'tool')
scene.remove_world_object('table')
scene.remove_world_object('box1')
scene.remove_world_object('box2')
scene.remove_world_object('target')
# Set the height of the table off the ground
table_ground = 0.3
# Set the length, width and height of the table
table_size = [0.2, 0.7, 0.01]
# Set the length, width and height of the object to attach
tool_size = [0.3, 0.02, 0.02]
# Create a pose for the tool relative to the end-effector
p = PoseStamped()
p.header.frame_id = end_eef
scene.attach_mesh
# Place the end of the object within the grasp of the gripper
p.pose.position.x = tool_size[0] / 2.0 - 0.025
p.pose.position.y = 0.0
p.pose.position.z = 0.0
# Align the object with the gripper (straight out)
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1
# Attach the tool to the end-effector
scene.attach_box(end_eef, 'tool', p, tool_size)
# Add a floating table top
table_pose = PoseStamped()
table_pose.header.frame_id = 'base'
table_pose.pose.position.x = 0.35
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box('table', table_pose, table_size)
# Update the current state
left_arm.set_start_state_to_current_state()
# Move the arm with the attached object to the 'straight_forward' position
left_arm.set_named_target('left_ready')
left_arm.go()
rospy.sleep(2)
# Return the arm in the "left_ready" pose stored in the SRDF file
left_arm.set_named_target('left_arm_zero')
left_arm.go()
rospy.sleep(2)
scene.remove_attached_object(end_eef, 'tool')
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
rospy.Subscriber('/aruco_single/pose', PoseStamped, self.pose_cb,queue_size=1)
scene=PlanningSceneInterface()
self.scene_pub=rospy.Publisher('planning_scene',PlanningScene)
self.colors=dict()
rospy.sleep(1)
arm=MoveGroupCommander('arm')
#gripper=MoveGroupCommander('gripper')
end_effector_link=arm.get_end_effector_link()
arm.set_goal_position_tolerance(0.005)
arm.set_goal_orientation_tolerance(0.025)
arm.allow_replanning(True)
#gripper.set_goal_position_tolerance(0.005)
#gripper.set_goal_orientation_tolerance(0.025)
#gripper.allow_replanning(True)
reference_frame='base_link'
arm.set_pose_reference_frame(reference_frame)
arm.set_planning_time(5)
#scene planning
table_id='table'
#cylinder_id='cylinder'
box2_id='box2'
target_id='target_object'
#scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(table_id)
scene.remove_world_object(target_id)
rospy.sleep(2)
table_ground=0.59
table_size=[0.5,1,0.01]
#box1_size=[0.1,0.05,0.03]
box2_size=[0.15,0.15,0.02]
r_tool_size=[0.05,0.04,0.22]
l_tool_size=[0.05,0.04,0.22]
target_size=[0.05,0.05,0.1]
table_pose=PoseStamped()
table_pose.header.frame_id=reference_frame
table_pose.pose.position.x=0.7
table_pose.pose.position.y=0.0
table_pose.pose.position.z=table_ground+table_size[2]/2.0
table_pose.pose.orientation.w=1.0
scene.add_box(table_id,table_pose,table_size)
'''
box1_pose=PoseStamped()
box1_pose.header.frame_id=reference_frame
box1_pose.pose.position.x=0.7
box1_pose.pose.position.y=-0.2
box1_pose.pose.position.z=table_ground+table_size[2]+box1_size[2]/2.0
box1_pose.pose.orientation.w=1.0
scene.add_box(box1_id,box1_pose,box1_size)
'''
box2_pose=PoseStamped()
box2_pose.header.frame_id=reference_frame
box2_pose.pose.position.x=0.55
box2_pose.pose.position.y=-0.12
box2_pose.pose.position.z=table_ground+table_size[2]+box2_size[2]/2.0
box2_pose.pose.orientation.w=1.0
scene.add_box(box2_id,box2_pose,box2_size)
target_pose=PoseStamped()
target_pose.header.frame_id=reference_frame
target_pose.pose.position.x=0.58
target_pose.pose.position.y=0.05
target_pose.pose.position.z=table_ground+table_size[2]+target_size[2]/2.0
target_pose.pose.orientation.x=0
target_pose.pose.orientation.y=0
target_pose.pose.orientation.z=0
target_pose.pose.orientation.w=1
scene.add_box(target_id,target_pose,target_size)
#left gripper
l_p=PoseStamped()
l_p.header.frame_id=end_effector_link
l_p.pose.position.x=0.00
l_p.pose.position.y=0.06
l_p.pose.position.z=0.11
l_p.pose.orientation.w=1
scene.attach_box(end_effector_link,'l_tool',l_p,l_tool_size)
#right gripper
r_p=PoseStamped()
r_p.header.frame_id=end_effector_link
r_p.pose.position.x=0.00
r_p.pose.position.y= -0.06
r_p.pose.position.z=0.11
r_p.pose.orientation.w=1
scene.attach_box(end_effector_link,'r_tool',r_p,r_tool_size)
#grasp
g_p=PoseStamped()
g_p.header.frame_id=end_effector_link
g_p.pose.position.x=0.00
g_p.pose.position.y= -0.00
g_p.pose.position.z=0.025
g_p.pose.orientation.w=0.707
g_p.pose.orientation.x=0
g_p.pose.orientation.y=-0.707
g_p.pose.orientation.z=0
self.setColor(table_id,0.8,0,0,1.0)
#self.setColor(box1_id,0.8,0.4,0,1.0)
self.setColor(box2_id,0.8,0.4,0,1.0)
self.setColor('r_tool',0.8,0,0)
self.setColor('l_tool',0.8,0,0)
self.setColor('target_object',0,1,0)
self.sendColors()
#motion planning
arm.set_named_target("initial_arm")
arm.go()
rospy.sleep(2)
grasp_pose=target_pose
grasp_pose.pose.position.x-=0.15
#grasp_pose.pose.position.z=
grasp_pose.pose.orientation.x=0
grasp_pose.pose.orientation.y=0.707
grasp_pose.pose.orientation.z=0
grasp_pose.pose.orientation.w=0.707
#arm.set_start_state_to_current_state()
'''
arm.set_pose_target(grasp_pose,end_effector_link)
traj=arm.plan()
arm.execute(traj)
print arm.get_current_joint_values()
'''
pre_joint_state=[0.16588150906995922, 1.7060146047438647, -0.00961761728757362, 1.8614674591892713, -2.9556667436476847, 1.7432451233907822, 3.1415]
arm.set_joint_value_target(pre_joint_state)
traj=arm.plan()
arm.execute(traj)
rospy.sleep(2)
arm.shift_pose_target(0,0.09,end_effector_link)
arm.go()
rospy.sleep(2)
scene.attach_box(end_effector_link,target_id,g_p,target_size)
rospy.sleep(2)
#grasping is over , from now is pouring
arm.shift_pose_target(2,0.15,end_effector_link)
arm.go()
rospy.sleep(2)
joint_state_1=arm.get_current_joint_values()
joint_state_1[0]-=0.17
arm.set_joint_value_target(joint_state_1)
arm.go()
rospy.sleep(1)
joint_state_2=arm.get_current_joint_values()
joint_state_2[6]-=1.8
arm.set_joint_value_target(joint_state_2)
arm.go()
rospy.sleep(1)
#print arm.get_current_joint_values()
#pouring test
for i in range(1,5):
joint_state_2[6]+=0.087
arm.set_joint_value_target(joint_state_2)
arm.go()
time.sleep(0.05)
joint_state_2[6]-=0.087
arm.set_joint_value_target(joint_state_2)
arm.go()
time.sleep(0.05)
print i
joint_state_2[6]+=1.8
arm.set_joint_value_target(joint_state_2)
arm.go()
rospy.sleep(2)
joint_state_1[0]+=0.17
arm.set_joint_value_target(joint_state_1)
arm.go()
rospy.sleep(1)
arm.shift_pose_target(2,-0.15,end_effector_link)
arm.go()
rospy.sleep(2)
scene.remove_attached_object(end_effector_link,target_id)
rospy.sleep(2)
arm.set_named_target("initial_arm")
arm.go()
rospy.sleep(2)
#remove and shut down
scene.remove_attached_object(end_effector_link,'l_tool')
rospy.sleep(1)
scene.remove_attached_object(end_effector_link,'r_tool')
rospy.sleep(1)
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def init():
global marker_array_pub, marker_pub, tf_broadcaster, tf_listener
global move_group, turntable
global camera_mesh, camera_pos, camera_size, min_distance, max_distance, num_positions, num_spins, object_size, photobox_pos, photobox_size, reach, simulation, test, turntable_pos, working_dir
rospy.init_node('acquisition')
camera_mesh = rospy.get_param('~camera_mesh', None)
camera_orientation = rospy.get_param('~camera_orientation', None)
camera_pos = rospy.get_param('~camera_pos', [0.0, 0.0, 0.0])
camera_size = rospy.get_param('~camera_size', [0.1, 0.1, 0.1])
min_distance = rospy.get_param('~min_distance', 0.2)
max_distance = rospy.get_param('~max_distance', min_distance)
max_velocity = rospy.get_param('~max_velocity', 0.1)
num_positions = rospy.get_param('~num_positions', 15)
num_spins = rospy.get_param('~num_spins', 8)
object_size = numpy.array(rospy.get_param('~object_size', [0.2, 0.2, 0.2]))
photobox_pos = rospy.get_param('~photobox_pos', [0.0, -0.6, 0.0])
photobox_size = rospy.get_param('~photobox_size', [0.7, 0.7, 1.0])
ptpip = rospy.get_param('~ptpip', None)
reach = rospy.get_param('~reach', 0.85)
simulation = '/gazebo' in get_node_names()
test = rospy.get_param('~test', True)
turntable_pos = rospy.get_param(
'~turntable_pos', photobox_pos[:2] + [photobox_pos[2] + 0.05])
turntable_radius = rospy.get_param('~turntable_radius', 0.2)
wall_thickness = rospy.get_param('~wall_thickness', 0.04)
marker_array_pub = rospy.Publisher('visualization_marker_array',
MarkerArray,
queue_size=1,
latch=True)
marker_pub = rospy.Publisher('visualization_marker',
Marker,
queue_size=1,
latch=True)
tf_broadcaster = tf.TransformBroadcaster()
tf_listener = tf.TransformListener()
move_group = MoveGroupCommander('manipulator')
move_group.set_max_acceleration_scaling_factor(
1.0 if simulation else max_velocity)
move_group.set_max_velocity_scaling_factor(
1.0 if simulation else max_velocity)
robot = RobotCommander()
scene = PlanningSceneInterface(synchronous=True)
try:
st_control = load_source(
'st_control',
os.path.join(RosPack().get_path('iai_scanning_table'), 'scripts',
'iai_scanning_table', 'st_control.py'))
turntable = st_control.ElmoUdp()
if turntable.check_device():
turntable.configure()
turntable.reset_encoder()
turntable.start_controller()
turntable.set_speed_deg(30)
except Exception as e:
print(e)
sys.exit('Could not connect to turntable.')
if simulation:
move_home()
elif not test:
working_dir = rospy.get_param('~working_dir', None)
if working_dir is None:
sys.exit('Working directory not specified.')
elif not camera.init(
os.path.join(os.path.dirname(os.path.abspath(__file__)), '..',
'out', working_dir, 'images'), ptpip):
sys.exit('Could not initialize camera.')
# add ground plane
ps = PoseStamped()
ps.header.frame_id = robot.get_planning_frame()
scene.add_plane('ground', ps)
# add photobox
ps.pose.position.x = photobox_pos[
0] + photobox_size[0] / 2 + wall_thickness / 2
ps.pose.position.y = photobox_pos[1]
ps.pose.position.z = photobox_pos[2] + photobox_size[2] / 2
scene.add_box('box_wall_left', ps,
(wall_thickness, photobox_size[1], photobox_size[2]))
ps.pose.position.x = photobox_pos[
0] - photobox_size[0] / 2 - wall_thickness / 2
ps.pose.position.y = photobox_pos[1]
ps.pose.position.z = photobox_pos[2] + photobox_size[2] / 2
scene.add_box('box_wall_right', ps,
(wall_thickness, photobox_size[1], photobox_size[2]))
ps.pose.position.x = photobox_pos[0]
ps.pose.position.y = photobox_pos[
1] - photobox_size[1] / 2 - wall_thickness / 2
ps.pose.position.z = photobox_pos[2] + photobox_size[2] / 2
scene.add_box('box_wall_back', ps,
(photobox_size[0], wall_thickness, photobox_size[2]))
ps.pose.position.x = photobox_pos[0]
ps.pose.position.y = photobox_pos[1]
ps.pose.position.z = photobox_pos[2] - wall_thickness / 2
scene.add_box('box_ground', ps,
(photobox_size[0], photobox_size[1], wall_thickness))
# add turntable
turntable_height = turntable_pos[2] - photobox_pos[2]
ps.pose.position.x = turntable_pos[0]
ps.pose.position.y = turntable_pos[1]
ps.pose.position.z = photobox_pos[2] + turntable_height / 2
scene.add_cylinder('turntable', ps, turntable_height, turntable_radius)
# add object on turntable
ps.pose.position.x = turntable_pos[0]
ps.pose.position.y = turntable_pos[1]
ps.pose.position.z = turntable_pos[2] + object_size[2] / 2
scene.add_cylinder('object', ps, object_size[2], max(object_size[:2]) / 2)
# add cable mounts
scene.remove_attached_object('upper_arm_link', 'upper_arm_cable_mount')
scene.remove_attached_object('forearm_link', 'forearm_cable_mount')
scene.remove_world_object('upper_arm_cable_mount')
scene.remove_world_object('forearm_cable_mount')
if ptpip is None and not simulation:
size = [0.08, 0.08, 0.08]
ps.header.frame_id = 'upper_arm_link'
ps.pose.position.x = -0.13
ps.pose.position.y = -0.095
ps.pose.position.z = 0.135
scene.attach_box(ps.header.frame_id, 'upper_arm_cable_mount', ps, size)
ps.header.frame_id = 'forearm_link'
ps.pose.position.x = -0.275
ps.pose.position.y = -0.08
ps.pose.position.z = 0.02
scene.attach_box(ps.header.frame_id, 'forearm_cable_mount', ps, size)
# add camera
eef_link = move_group.get_end_effector_link()
ps = PoseStamped()
ps.header.frame_id = eef_link
scene.remove_attached_object(eef_link, 'camera_0')
scene.remove_attached_object(eef_link, 'camera_1')
scene.remove_world_object('camera_0')
scene.remove_world_object('camera_1')
ps.pose.position.y = camera_pos[1]
ps.pose.position.z = camera_pos[2]
if camera_mesh:
ps.pose.position.x = camera_pos[0]
quaternion = tf.transformations.quaternion_from_euler(
math.radians(camera_orientation[0]),
math.radians(camera_orientation[1]),
math.radians(camera_orientation[2]))
ps.pose.orientation.x = quaternion[0]
ps.pose.orientation.y = quaternion[1]
ps.pose.orientation.z = quaternion[2]
ps.pose.orientation.w = quaternion[3]
scene.attach_mesh(eef_link, 'camera_0', ps,
os.path.expanduser(camera_mesh), camera_size)
if not simulation:
scene.attach_mesh(eef_link, 'camera_1', ps,
os.path.expanduser(camera_mesh),
numpy.array(camera_size) * 1.5)
vertices = scene.get_attached_objects(
['camera_0'])['camera_0'].object.meshes[0].vertices
camera_size[0] = max(vertice.x for vertice in vertices) - min(
vertice.x for vertice in vertices)
camera_size[1] = max(vertice.y for vertice in vertices) - min(
vertice.y for vertice in vertices)
camera_size[2] = max(vertice.z for vertice in vertices) - min(
vertice.z for vertice in vertices)
else:
ps.pose.position.x = camera_pos[0] + camera_size[0] / 2
scene.attach_box(eef_link, 'camera_0', ps, camera_size)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=5)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose',
PoseStamped,
queue_size=5)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the right arm
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.05)
right_arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
right_arm.set_planning_time(60)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 5
# Set a limit on the number of place attempts
max_place_attempts = 5
# Give the scene a chance to catch up
rospy.sleep(2)
# Give each of the scene objects a unique name
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
target_id = 'target'
tool_id = 'tool'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(target_id)
scene.remove_world_object(tool_id)
# Remove any attached objects from a previous session
scene.remove_attached_object(GRIPPER_FRAME, target_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "grasp" pose stored in the SRDF file
right_arm.set_named_target('right_arm_up')
right_arm.go()
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_OPEN)
right_gripper.go()
rospy.sleep(5)
# Set the height of the table off the ground
table_ground = 0.04
# Set the dimensions of the scene objects [l, w, h]
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# Set the target size [l, w, h]
target_size = [0.02, 0.01, 0.12]
target_x = 0.135
#target_y = -0.32
target_y = -0.285290879999
# Add a table top and two boxes to the scene
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose.position.x = 0.25
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
# Set the target pose in between the boxes and on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = target_x
target_pose.pose.position.y = target_y
target_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
target_pose.pose.orientation.w = 1.0
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
# Make the table blue and the boxes orange
self.setColor(table_id, 0, 0, 0.8, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# Make the target yellow
self.setColor(target_id, 0.9, 0.9, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the support surface name to the table object
right_arm.set_support_surface_name(table_id)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.18
place_pose.pose.position.y = 0
place_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
place_pose.pose.orientation.w = 1.0
p = PoseStamped()
p.header.frame_id = "up1_footprint"
p.pose.position.x = 0.12792118579
p.pose.position.y = -0.285290879999
p.pose.position.z = 0.120301181892
p.pose.orientation.x = 0.0
p.pose.orientation.y = 0.0
p.pose.orientation.z = -0.706825181105
p.pose.orientation.w = 0.707388269167
right_gripper.set_pose_target(p.pose)
# pick an object
right_arm.allow_replanning(True)
right_arm.allow_looking(True)
right_arm.set_goal_tolerance(0.05)
right_arm.set_planning_time(60)
print "arm grasp"
success = 0
attempt = 0
while not success:
p_plan = right_arm.plan()
attempt = attempt + 1
print "Planning attempt: " + str(attempt)
if p_plan.joint_trajectory.points != []:
success = 1
print "arm grasp"
right_arm.execute(p_plan)
rospy.sleep(5)
right_gripper.set_joint_value_target(GRIPPER_GRASP)
right_gripper.go()
print "gripper closed"
rospy.sleep(5)
scene.attach_box(GRIPPER_FRAME, target_id)
print "object attached"
right_arm.set_named_target('right_arm_up')
right_arm.go()
print "arm up"
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
class MoveItDemo:
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
#Initialize robot
robot = moveit_commander.RobotCommander()
# Use the planning scene object to add or remove objects
self.scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10)
# Create a publisher for displaying object frames
self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10)
### Create a publisher for visualizing direction ###
self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the MoveIt! commander for the arm
self.right_arm = MoveGroupCommander(GROUP_NAME_ARM)
self.left_arm = MoveGroupCommander('left_arm')
# Initialize the MoveIt! commander for the gripper
self.right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
self.left_gripper = MoveGroupCommander('left_gripper')
# eel = len(self.right_arm.get_end_effector_link())
# print eel
# Allow 5 seconds per planning attempt
# self.right_arm.set_planning_time(5)
# Prepare Action Controller for gripper
self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction)
self.ac.wait_for_server()
# Give the scene a chance to catch up
rospy.sleep(2)
# Prepare Gazebo Subscriber
self.pwh = None
self.pwh_copy = None
self.idx_targ = None
self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)
### OPEN THE GRIPPER ###
self.open_gripper()
# PREPARE THE SCENE
while self.pwh is None:
rospy.sleep(0.05)
### Attach / Remove Object Flag ###
self.aro = None
# Run and keep in the BG the scene generator with ctrl^c kill ###
timerThread = threading.Thread(target=self.scene_generator)
timerThread.daemon = True
timerThread.start()
## Give some time to ensure the thread starts!! ##
rospy.sleep(5)
### GENERATE THE BLACKLIST AND REMOVE ATTACHED OBJECTS FROM PREVIOUS RUNS ###
self.idx_list = self.bl()
### GIVE SCENE TIME TO CATCH UP ###
rospy.sleep(5)
################################## GRASP EXECUTION #####################################
print "==================== Executing ==========================="
start_time = time.time()
### PERSONAL REMINDER!!! WHAT IS WHAT!!! ###
# print obj_id[obj_id.index('target')]
# print obj_id.index('target')
### MOVE LEFT ARM OUT OF THE WAY ###
self.lasp()
success = False
while success == False and len(self.idx_list)>0:
success, pgr_target = self.grasp_attempt()
print ("GA Returns:", success)
if success is not False:
self.flag = 0 # To let the planning scene know when to remove the object
self.post_grasp(pgr_target, obj_id.index('target'),'true')
self.place_object(obj_id.index('target'))
break
else:
idx = self.idx_list[0]
ds, pgr_col_obj = self.declutter_scene(idx)
print ("DS Returns:", ds)
if ds == True:
self.flag = 0 # To let the planning scene know when to remove the object
self.post_grasp(pgr_col_obj, obj_id.index(obj_id[idx]),'true')
self.place_object(obj_id.index(obj_id[idx]))
self.idx_list.pop(0)
print "==================== THE END! ======================"
print("--- %s seconds ---" % (time.time() - start_time))
rospy.sleep(5)
# # Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# # Exit the script
moveit_commander.os._exit(0)
################################################################# FUNCTIONS #################################################################################
def grasp_attempt(self):
# start_time = time.time()
retreat = None
init_poses = []
grasp_poses = []
for axis in range(0,6):
# while obj_id[obj_id.index('target')] is not 'target':
# print '!!!!!'
# rospy.sleep(0.05)
pg = self.grasp_pose(obj_pose[obj_id.index('target')], axis, 'pg')
gp = self.grasp_pose(obj_pose[obj_id.index('target')], axis, 'gp')
init_poses.append(pg)
grasp_poses.append(gp)
pre_grasps = self.grasp_generator(init_poses)
grasps = self.grasp_generator(grasp_poses)
for grasp in grasps:
self.gripper_pose_pub.publish(grasp)
rospy.sleep(0.05)
success = False
i = 1
for pg, gr in izip(pre_grasps, grasps):
self.gripper_pose_pub.publish(gr)
print ("G Attempt: ", i)
plp = self.right_arm.plan(pg.pose)
if len(plp.joint_trajectory.points) == 0:
print "No valid pregrasp Position, continuing on next one"
i+=1
continue
i+=1
self.right_arm.plan(pg.pose)
self.right_arm.go(wait=True)
rospy.sleep(5)
plg = self.right_arm.plan(gr.pose)
if len(plg.joint_trajectory.points) >= 10:
self.right_arm.go()
success = True
retreat = gr
print "Grasping"
break
# print("--- %s seconds ---" % (time.time() - start_time))
return success , retreat
def declutter_scene(self,index):
retreat = None
init_poses = []
grasp_poses = []
for axis in range(0,6):
pg = self.grasp_pose(obj_pose[index], axis, 'pg')
gp = self.grasp_pose(obj_pose[index], axis, 'gp')
init_poses.append(pg)
grasp_poses.append(gp)
pre_grasps = self.grasp_generator(init_poses)
grasps = self.grasp_generator(grasp_poses)
for grasp in grasps:
self.gripper_pose_pub.publish(grasp)
rospy.sleep(0.05)
success = False
i= 1
for pg, gr in izip(pre_grasps, grasps):
plp = self.right_arm.plan(pg.pose)
print (" DS Attempt: ", i)
self.gripper_pose_pub.publish(gr)
self.right_arm.plan(pg.pose)
if len(plp.joint_trajectory.points) == 0:
print "No valid pregrasp Position, continuing on next one"
i+=1
continue
i+=1
self.right_arm.plan(pg.pose)
self.right_arm.go()
rospy.sleep(5)
plg = self.right_arm.plan(gr.pose)
if len(plg.joint_trajectory.points) >= 10:
self.right_arm.go()
print "Grasping"
success = True
retreat = gr
break
return success, retreat
def place_object(self, obj_idx):
self.aro = obj_idx
### GENERATE PLACE POSES ###
places = self.place_generator()
### TRY THESE POSES ###
i = 1
for place in places:
print (" Place Attempt: ", i)
plpl = self.right_arm.plan(place.pose)
print len(plpl.joint_trajectory.points)
if len(plpl.joint_trajectory.points) == 0:
i+=1
continue
self.right_arm.plan(plpl)
self.right_arm.go(wait=True)
### INFORM SCENE ###
# self.open_gripper()
# self.aro = None
### RETURN HAND TO STARTING POSITION ###
self.post_grasp(place,obj_idx, 'false')
self.rasp()
break
def post_grasp(self,new_pose, obj_idx, fl):
######### GRASP OBJECT/ REMOVE FROM SCENE ######.
if fl == 'true':
self.close_gripper()
self.aro = obj_idx
else:
self.open_gripper()
self.aro = None
rospy.sleep(2)
### POST GRASP RETREAT ###
M1 = transformations.quaternion_matrix([new_pose.pose.orientation.x, new_pose.pose.orientation.y, new_pose.pose.orientation.z, new_pose.pose.orientation.w])
M1[0,3] = new_pose.pose.position.x
M1[1,3] = new_pose.pose.position.y
M1[2,3] = new_pose.pose.position.z
M2 = transformations.euler_matrix(0, 0, 0)
M2[0,3] = 0.0 # offset about x
M2[1,3] = 0.0 # about y
M2[2,3] = 0.25 # about z
T1 = np.dot(M2, M1)
npo = deepcopy(new_pose)
npo.pose.position.x = T1[0,3]
npo.pose.position.y = T1[1,3]
npo.pose.position.z = T1[2,3]
quat = transformations.quaternion_from_matrix(T1)
npo.pose.orientation.x = quat[0]
npo.pose.orientation.y = quat[1]
npo.pose.orientation.z = quat[2]
npo.pose.orientation.w = quat[3]
npo.header.frame_id = REFERENCE_FRAME
self.right_arm.plan(npo.pose)
self.right_arm.go(wait=True)
def place_generator(self):
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.57
place_pose.pose.position.y = 0.16
place_pose.pose.position.z = 0.56
place_pose.pose.orientation.w = 1.0
P = transformations.quaternion_matrix([place_pose.pose.orientation.x, place_pose.pose.orientation.y, place_pose.pose.orientation.z, place_pose.pose.orientation.w])
P[0,3] = place_pose.pose.position.x
P[1,3] = place_pose.pose.position.y
P[2,3] = place_pose.pose.position.z
places =[]
yaw_angles = [0, 1,57, -1,57 , 3,14]
x_vals = [0, 0.05 ,0.1 , 0.15]
z_vals = [0.05 ,0.1 , 0.15]
for y in yaw_angles:
G = transformations.euler_matrix(0, 0, y)
G[0,3] = 0.0 # offset about x
G[1,3] = 0.0 # about y
G[2,3] = 0.0 # about z
for z in z_vals:
for x in x_vals:
TM = np.dot(P, G)
pl = deepcopy(place_pose)
pl.pose.position.x = TM[0,3] +x
pl.pose.position.y = TM[1,3]
pl.pose.position.z = TM[2,3] +z
quat = transformations.quaternion_from_matrix(TM)
pl.pose.orientation.x = quat[0]
pl.pose.orientation.y = quat[1]
pl.pose.orientation.z = quat[2]
pl.pose.orientation.w = quat[3]
pl.header.frame_id = REFERENCE_FRAME
places.append(deepcopy(pl))
return places
def grasp_pose(self, target_pose, axis, stage):
############ TODO : GENERATE AUTOMATED PRE-GRASPING POSITIONS BASED ON THE PRIMITIVE #########
if axis == 0:
M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M1[0,3] = target_pose.pose.position.x
M1[1,3] = target_pose.pose.position.y
M1[2,3] = target_pose.pose.position.z
M2 = transformations.euler_matrix(0, 0, 0)
if stage == 'pg':
M2[0,3] = -0.25 # offset about x
elif stage == 'gp':
M2[0,3] = -0.18 # offset about x
M2[1,3] = 0.0 # about y
M2[2,3] = 0.0 # about z
elif axis == 1:
M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M1[0,3] = target_pose.pose.position.x
M1[1,3] = target_pose.pose.position.y
M1[2,3] = target_pose.pose.position.z
M2 = transformations.euler_matrix(0, 0, 1.57)
M2[0,3] = 0.0 # offset about x
if stage == 'pg':
M2[1,3] = -0.25 # about y
elif stage == 'gp':
M2[1,3] = -0.18 # about y
M2[2,3] = 0.0 # about z
elif axis == 2:
M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M1[0,3] = target_pose.pose.position.x
M1[1,3] = target_pose.pose.position.y
M1[2,3] = target_pose.pose.position.z
M2 = transformations.euler_matrix(0, 0, -1.57)
M2[0,3] = 0.0 # offset about x
if stage == 'pg':
M2[1,3] = 0.25 # about y
elif stage == 'gp':
M2[1,3] = 0.18 # about y
M2[2,3] = 0.0 # about z
elif axis == 3:
M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M1[0,3] = target_pose.pose.position.x
M1[1,3] = target_pose.pose.position.y
M1[2,3] = target_pose.pose.position.z
M2 = transformations.euler_matrix(0, 0, 3.14)
if stage == 'pg':
M2[0,3] = 0.25 # offset about x
elif stage == 'gp':
M2[0,3] = 0.18 # offset about x
M2[1,3] = 0.0 # about y
M2[2,3] = 0.0 # about z
elif axis == 4:
M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M1[0,3] = target_pose.pose.position.x
M1[1,3] = target_pose.pose.position.y
M1[2,3] = target_pose.pose.position.z
M2 = transformations.euler_matrix(0, 1.57, 0)
M2[0,3] = 0.0 # offset about x
M2[1,3] = 0.0 # about y
if stage == 'pg':
M2[2,3] = 0.30 # about z
elif stage == 'gp':
M2[2,3] = 0.23 # about z
else:
M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M1[0,3] = target_pose.pose.position.x
M1[1,3] = target_pose.pose.position.y
M1[2,3] = target_pose.pose.position.z
M2 = transformations.euler_matrix(1.57, 1.57, 0)
M2[0,3] = 0.0 # offset about x
M2[1,3] = 0.0 # about y
if stage == 'pg':
M2[2,3] = 0.30 # about z
elif stage == 'gp':
M2[2,3] = 0.23 # about z
T1 = np.dot(M1, M2)
grasp_pose = deepcopy(target_pose)
grasp_pose.pose.position.x = T1[0,3]
grasp_pose.pose.position.y = T1[1,3]
grasp_pose.pose.position.z = T1[2,3]
quat = transformations.quaternion_from_matrix(T1)
grasp_pose.pose.orientation.x = quat[0]
grasp_pose.pose.orientation.y = quat[1]
grasp_pose.pose.orientation.z = quat[2]
grasp_pose.pose.orientation.w = quat[3]
grasp_pose.header.frame_id = REFERENCE_FRAME
return grasp_pose
def grasp_generator(self, initial_poses):
# A list to hold the grasps
grasps = []
o = [] # Original Pose of the object (o)
O=[]
i= 0
while i < len(initial_poses):
o.append(initial_poses[i])
i+=1
G = transformations.euler_matrix(0, 0, 0)
# Generate a grasps for along z axis (x and y directions)
k = 0
while k <= 5:
O.append(transformations.quaternion_matrix([o[k].pose.orientation.x, o[k].pose.orientation.y, o[k].pose.orientation.z, o[k].pose.orientation.w]))
O[k][0,3] = o[k].pose.position.x
O[k][1,3] = o[k].pose.position.y
O[k][2,3] = o[k].pose.position.z
if k in range(0,4):
for z in self.drange(0.005-obj_size[obj_id.index('target')][2]/2,-0.005 + obj_size[obj_id.index('target')][2]/2, 0.02): ### TODO: USE EACH OBJECTS SIZE NOT ONLY THE TARGETS ###
# print z
T = np.dot(O[k], G)
grasp = deepcopy(o[k])
grasp.pose.position.x = T[0,3]
grasp.pose.position.y = T[1,3]
grasp.pose.position.z = T[2,3] +z
quat = transformations.quaternion_from_matrix(T)
grasp.pose.orientation.x = quat[0]
grasp.pose.orientation.y = quat[1]
grasp.pose.orientation.z = quat[2]
grasp.pose.orientation.w = quat[3]
grasp.header.frame_id = REFERENCE_FRAME
# Append the grasp to the list
grasps.append(deepcopy(grasp))
elif k == 4:
for x in self.drange(-obj_size[obj_id.index('target')][1]/2, obj_size[obj_id.index('target')][1]/2, 0.02):
# print z
T = np.dot(O[k], G)
grasp = deepcopy(o[k])
grasp.pose.position.x = T[0,3] +x
grasp.pose.position.y = T[1,3]
grasp.pose.position.z = T[2,3]
quat = transformations.quaternion_from_matrix(T)
grasp.pose.orientation.x = quat[0]
grasp.pose.orientation.y = quat[1]
grasp.pose.orientation.z = quat[2]
grasp.pose.orientation.w = quat[3]
grasp.header.frame_id = REFERENCE_FRAME
# Append the grasp to the list
grasps.append(deepcopy(grasp))
else:
for y in self.drange(-obj_size[obj_id.index('target')][1]/2, obj_size[obj_id.index('target')][1]/2, 0.02):
# print z
T = np.dot(O[k], G)
grasp = deepcopy(o[k])
grasp.pose.position.x = T[0,3]
grasp.pose.position.y = T[1,3] +y
grasp.pose.position.z = T[2,3]
quat = transformations.quaternion_from_matrix(T)
grasp.pose.orientation.x = quat[0]
grasp.pose.orientation.y = quat[1]
grasp.pose.orientation.z = quat[2]
grasp.pose.orientation.w = quat[3]
grasp.header.frame_id = REFERENCE_FRAME
# Append the grasp to the list
grasps.append(deepcopy(grasp))
k+=1
print len(grasps)
# Return the list
return grasps
def scene_generator(self):
while True:
# print "happening"
obj_pose =[]
obj_id = []
obj_size = []
bl = ['ground_plane','pr2']
global obj_pose, obj_id , obj_size
ops = PoseStamped()
ops.header.frame_id = REFERENCE_FRAME
for model_name in self.pwh.name:
if model_name not in bl:
obj_id.append(model_name)
ops.pose = self.pwh.pose[self.pwh.name.index(model_name)]
obj_pose.append(deepcopy(ops))
obj_size.append([0.05, 0.05, 0.15])
# obj_id[obj_id.index('custom_1')] = 'target'
obj_size[obj_id.index('custom_2')] = [0.05, 0.05, 0.10]
obj_size[obj_id.index('custom_3')] = [0.05, 0.05, 0.05]
obj_size[obj_id.index('custom_table')] = [1.5, 0.8, 0.03]
if self.aro is None:
for i in range(0, len(obj_id)):
### CREATE THE SCENE ###
self.scene.add_box(obj_id[i], obj_pose[i], obj_size[i])
self.setColor(obj_id[i], 1, 0.623, 0, 1.0)
### Make the target purple and table green ###
self.setColor(obj_id[obj_id.index('target')], 0.6, 0, 1, 1.0)
self.setColor(obj_id[obj_id.index('custom_table')], 0.3, 1, 0.3, 1.0)
self.scene.remove_attached_object(GRIPPER_FRAME)
# Send the colors to the planning scene
self.sendColors()
else:
if self.flag == 0:
touch_links = [GRIPPER_FRAME, 'r_gripper_l_finger_tip_link','r_gripper_r_finger_tip_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link', 'r_wrist_roll_link', 'r_upper_arm_link']
#print touch_links
self.scene.attach_box(GRIPPER_FRAME, obj_id[self.aro], obj_pose[self.aro], obj_size[self.aro], touch_links)
### REMOVE SPECIFIC OBJECT AFTER IT HAS BEEN ATTACHED TO GRIPPER ###
self.scene.remove_world_object(obj_id[self.aro])
self.flag +=1
time.sleep(0.5)
def model_state_callback(self,msg):
self.pwh = ModelStates()
self.pwh = msg
def bl(self):
blist = ['target','custom_2','custom_3', 'custom_table']
self.blist = []
for name in obj_id:
if name not in blist:
self.blist.append(obj_id.index(name))
# Remove any attached objects from a previous session
self.scene.remove_attached_object(GRIPPER_FRAME, obj_id[obj_id.index(name)])
self.scene.remove_attached_object(GRIPPER_FRAME, 'target')
return self.blist
def drange(self, start, stop, step):
r = start
while r < stop:
yield r
r += step
def close_gripper(self):
g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.044, 100))
self.ac.send_goal(g_close)
self.ac.wait_for_result()
rospy.sleep(15) # Gazebo requires up to 15 seconds to attach object
def open_gripper(self):
g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.0899, 100))
self.ac.send_goal(g_open)
self.ac.wait_for_result()
rospy.sleep(5) # And up to 20 to detach it
# Set the color of an object
def setColor(self, name, r, g, b, a = 0.9):
# Initialize a MoveIt color object
color = ObjectColor()
# Set the id to the name given as an argument
color.id = name
# Set the rgb and alpha values given as input
color.color.r = r
color.color.g = g
color.color.b = b
color.color.a = a
# Update the global color dictionary
self.colors[name] = color
# Actually send the colors to MoveIt!
def sendColors(self):
# Initialize a planning scene object
p = PlanningScene()
# Need to publish a planning scene diff
p.is_diff = True
# Append the colors from the global color dictionary
for color in self.colors.values():
p.object_colors.append(color)
# Publish the scene diff
self.scene_pub.publish(p)
def lasp(self):
sp = PoseStamped()
sp.header.frame_id = REFERENCE_FRAME
sp.pose.position.x = 0.3665
sp.pose.position.y = 0.74094
sp.pose.position.z = 1.1449
sp.pose.orientation.x = 0.80503
sp.pose.orientation.y = -0.18319
sp.pose.orientation.z = 0.31988
sp.pose.orientation.w = 0.46481
self.left_arm.plan(sp)
self.left_arm.go(wait=True)
def rasp(self):
sp = PoseStamped()
sp.header.frame_id = REFERENCE_FRAME
sp.pose.position.x = 0.39571
sp.pose.position.y = -0.40201
sp.pose.position.z = 1.1128
sp.pose.orientation.x =0.00044829
sp.pose.orientation.y = 0.57956
sp.pose.orientation.z = 9.4878e-05
sp.pose.orientation.w = 0.81493
self.right_arm.plan(sp)
self.right_arm.go(wait=True)
class MoveGroupCommandInterpreter(object):
"""
Interpreter for simple commands
"""
DEFAULT_FILENAME = "move_group.cfg"
GO_DIRS = {
"up": (2, 1),
"down": (2, -1),
"z": (2, 1),
"left": (1, 1),
"right": (1, -1),
"y": (1, 1),
"forward": (0, 1),
"backward": (0, -1),
"back": (0, -1),
"x": (0, 1),
}
def __init__(self):
self._gdict = {}
self._group_name = ""
self._prev_group_name = ""
self._planning_scene_interface = PlanningSceneInterface()
self._robot = RobotCommander()
self._last_plan = None
self._db_host = None
self._db_port = 33829
self._trace = False
def get_active_group(self):
if len(self._group_name) > 0:
return self._gdict[self._group_name]
else:
return None
def get_loaded_groups(self):
return self._gdict.keys()
def execute(self, cmd):
cmd = self.resolve_command_alias(cmd)
result = self.execute_generic_command(cmd)
if result != None:
return result
else:
if len(self._group_name) > 0:
return self.execute_group_command(
self._gdict[self._group_name], cmd)
else:
return (
MoveGroupInfoLevel.INFO,
self.get_help() +
"\n\nNo groups initialized yet. You must call the 'use' or the 'load' command first.\n",
)
def execute_generic_command(self, cmd):
if os.path.isfile("cmd/" + cmd):
cmd = "load cmd/" + cmd
cmdlow = cmd.lower()
if cmdlow.startswith("use"):
if cmdlow == "use":
return (MoveGroupInfoLevel.INFO,
"\n".join(self.get_loaded_groups()))
clist = cmd.split()
clist[0] = clist[0].lower()
if len(clist) == 2:
if clist[0] == "use":
if clist[1] == "previous":
clist[1] = self._prev_group_name
if len(clist[1]) == 0:
return (MoveGroupInfoLevel.DEBUG, "OK")
if clist[1] in self._gdict:
self._prev_group_name = self._group_name
self._group_name = clist[1]
return (MoveGroupInfoLevel.DEBUG, "OK")
else:
try:
mg = MoveGroupCommander(clist[1])
self._gdict[clist[1]] = mg
self._group_name = clist[1]
return (MoveGroupInfoLevel.DEBUG, "OK")
except MoveItCommanderException as e:
return (
MoveGroupInfoLevel.FAIL,
"Initializing " + clist[1] + ": " + str(e),
)
except:
return (
MoveGroupInfoLevel.FAIL,
"Unable to initialize " + clist[1],
)
elif cmdlow.startswith("trace"):
if cmdlow == "trace":
return (
MoveGroupInfoLevel.INFO,
"trace is on" if self._trace else "trace is off",
)
clist = cmdlow.split()
if clist[0] == "trace" and clist[1] == "on":
self._trace = True
return (MoveGroupInfoLevel.DEBUG, "OK")
if clist[0] == "trace" and clist[1] == "off":
self._trace = False
return (MoveGroupInfoLevel.DEBUG, "OK")
elif cmdlow.startswith("load"):
filename = self.DEFAULT_FILENAME
clist = cmd.split()
if len(clist) > 2:
return (MoveGroupInfoLevel.WARN,
"Unable to parse load command")
if len(clist) == 2:
filename = clist[1]
if not os.path.isfile(filename):
filename = "cmd/" + filename
line_num = 0
line_content = ""
try:
f = open(filename, "r")
for line in f:
line_num += 1
line = line.rstrip()
line_content = line
if self._trace:
print("%s:%d: %s" %
(filename, line_num, line_content))
comment = line.find("#")
if comment != -1:
line = line[0:comment].rstrip()
if line != "":
self.execute(line.rstrip())
line_content = ""
return (MoveGroupInfoLevel.DEBUG, "OK")
except MoveItCommanderException as e:
if line_num > 0:
return (
MoveGroupInfoLevel.WARN,
"Error at %s:%d: %s\n%s" %
(filename, line_num, line_content, str(e)),
)
else:
return (
MoveGroupInfoLevel.WARN,
"Processing " + filename + ": " + str(e),
)
except:
if line_num > 0:
return (
MoveGroupInfoLevel.WARN,
"Error at %s:%d: %s" %
(filename, line_num, line_content),
)
else:
return (MoveGroupInfoLevel.WARN,
"Unable to load " + filename)
elif cmdlow.startswith("save"):
filename = self.DEFAULT_FILENAME
clist = cmd.split()
if len(clist) > 2:
return (MoveGroupInfoLevel.WARN,
"Unable to parse save command")
if len(clist) == 2:
filename = clist[1]
try:
f = open(filename, "w")
for gr in self._gdict.keys():
f.write("use " + gr + "\n")
known = self._gdict[gr].get_remembered_joint_values()
for v in known.keys():
f.write(v + " = [" +
" ".join([str(x) for x in known[v]]) + "]\n")
if self._db_host != None:
f.write("database " + self._db_host + " " +
str(self._db_port) + "\n")
return (MoveGroupInfoLevel.DEBUG, "OK")
except:
return (MoveGroupInfoLevel.WARN, "Unable to save " + filename)
else:
return None
def execute_group_command(self, g, cmdorig):
cmd = cmdorig.lower()
if cmd == "stop":
g.stop()
return (MoveGroupInfoLevel.DEBUG, "OK")
if cmd == "id":
return (MoveGroupInfoLevel.INFO, g.get_name())
if cmd == "help":
return (MoveGroupInfoLevel.INFO, self.get_help())
if cmd == "vars":
known = g.get_remembered_joint_values()
return (MoveGroupInfoLevel.INFO,
"[" + " ".join(known.keys()) + "]")
if cmd == "joints":
joints = g.get_joints()
return (
MoveGroupInfoLevel.INFO,
"\n" + "\n".join(
[str(i) + " = " + joints[i]
for i in range(len(joints))]) + "\n",
)
if cmd == "show":
return self.command_show(g)
if cmd == "current":
return self.command_current(g)
if cmd == "ground":
pose = PoseStamped()
pose.pose.position.x = 0
pose.pose.position.y = 0
# offset such that the box is 0.1 mm below ground (to prevent collision with the robot itself)
pose.pose.position.z = -0.0501
pose.pose.orientation.x = 0
pose.pose.orientation.y = 0
pose.pose.orientation.z = 0
pose.pose.orientation.w = 1
pose.header.stamp = rospy.get_rostime()
pose.header.frame_id = self._robot.get_root_link()
self._planning_scene_interface.attach_box(
self._robot.get_root_link(), "ground", pose, (3, 3, 0.1))
return (MoveGroupInfoLevel.INFO, "Added ground")
if cmd == "eef":
if len(g.get_end_effector_link()) > 0:
return (MoveGroupInfoLevel.INFO, g.get_end_effector_link())
else:
return (MoveGroupInfoLevel.INFO,
"There is no end effector defined")
if cmd == "database":
if self._db_host == None:
return (MoveGroupInfoLevel.INFO, "Not connected to a database")
else:
return (
MoveGroupInfoLevel.INFO,
"Connected to " + self._db_host + ":" + str(self._db_port),
)
if cmd == "plan":
if self._last_plan != None:
return (MoveGroupInfoLevel.INFO, str(self._last_plan))
else:
return (MoveGroupInfoLevel.INFO, "No previous plan")
if cmd == "constrain":
g.clear_path_constraints()
return (MoveGroupInfoLevel.SUCCESS, "Cleared path constraints")
if cmd == "tol" or cmd == "tolerance":
return (
MoveGroupInfoLevel.INFO,
"Joint = %0.6g, Position = %0.6g, Orientation = %0.6g" %
g.get_goal_tolerance(),
)
if cmd == "time":
return (MoveGroupInfoLevel.INFO, str(g.get_planning_time()))
if cmd == "execute":
if self._last_plan != None:
if g.execute(self._last_plan):
return (MoveGroupInfoLevel.SUCCESS,
"Plan submitted for execution")
else:
return (
MoveGroupInfoLevel.WARN,
"Failed to submit plan for execution",
)
else:
return (MoveGroupInfoLevel.WARN, "No motion plan computed")
# see if we have assignment between variables
assign_match = re.match(r"^(\w+)\s*=\s*(\w+)$", cmd)
if assign_match:
known = g.get_remembered_joint_values()
if assign_match.group(2) in known:
g.remember_joint_values(assign_match.group(1),
known[assign_match.group(2)])
return (
MoveGroupInfoLevel.SUCCESS,
assign_match.group(1) + " is now the same as " +
assign_match.group(2),
)
else:
return (MoveGroupInfoLevel.WARN,
"Unknown command: '" + cmd + "'")
# see if we have assignment of matlab-like vector syntax
set_match = re.match(r"^(\w+)\s*=\s*\[([\d\.e\-\+\s]+)\]$", cmd)
if set_match:
try:
g.remember_joint_values(
set_match.group(1),
[float(x) for x in set_match.group(2).split()])
return (
MoveGroupInfoLevel.SUCCESS,
"Remembered joint values [" + set_match.group(2) +
"] under the name " + set_match.group(1),
)
except:
return (
MoveGroupInfoLevel.WARN,
"Unable to parse joint value [" + set_match.group(2) + "]",
)
# see if we have assignment of matlab-like element update
component_match = re.match(
r"^(\w+)\s*\[\s*(\d+)\s*\]\s*=\s*([\d\.e\-\+]+)$", cmd)
if component_match:
known = g.get_remembered_joint_values()
if component_match.group(1) in known:
try:
val = known[component_match.group(1)]
val[int(component_match.group(2))] = float(
component_match.group(3))
g.remember_joint_values(component_match.group(1), val)
return (
MoveGroupInfoLevel.SUCCESS,
"Updated " + component_match.group(1) + "[" +
component_match.group(2) + "]",
)
except:
return (
MoveGroupInfoLevel.WARN,
"Unable to parse index or value in '" + cmd + "'",
)
else:
return (MoveGroupInfoLevel.WARN,
"Unknown command: '" + cmd + "'")
clist = cmdorig.split()
clist[0] = clist[0].lower()
# if this is an unknown one-word command, it is probably a variable
if len(clist) == 1:
known = g.get_remembered_joint_values()
if cmd in known:
return (
MoveGroupInfoLevel.INFO,
"[" + " ".join([str(x) for x in known[cmd]]) + "]",
)
else:
return (MoveGroupInfoLevel.WARN,
"Unknown command: '" + cmd + "'")
# command with one argument
if len(clist) == 2:
if clist[0] == "go":
self._last_plan = None
if clist[1] == "rand" or clist[1] == "random":
vals = g.get_random_joint_values()
g.set_joint_value_target(vals)
if g.go():
return (
MoveGroupInfoLevel.SUCCESS,
"Moved to random target [" +
" ".join([str(x) for x in vals]) + "]",
)
else:
return (
MoveGroupInfoLevel.FAIL,
"Failed while moving to random target [" +
" ".join([str(x) for x in vals]) + "]",
)
else:
try:
g.set_named_target(clist[1])
if g.go():
return (MoveGroupInfoLevel.SUCCESS,
"Moved to " + clist[1])
else:
return (
MoveGroupInfoLevel.FAIL,
"Failed while moving to " + clist[1],
)
except MoveItCommanderException as e:
return (
MoveGroupInfoLevel.WARN,
"Going to " + clist[1] + ": " + str(e),
)
except:
return (MoveGroupInfoLevel.WARN,
clist[1] + " is unknown")
if clist[0] == "plan":
self._last_plan = None
vals = None
if clist[1] == "rand" or clist[1] == "random":
vals = g.get_random_joint_values()
g.set_joint_value_target(vals)
self._last_plan = g.plan()[1] # The trajectory msg
else:
try:
g.set_named_target(clist[1])
self._last_plan = g.plan()[1] # The trajectory msg
except MoveItCommanderException as e:
return (
MoveGroupInfoLevel.WARN,
"Planning to " + clist[1] + ": " + str(e),
)
except:
return (MoveGroupInfoLevel.WARN,
clist[1] + " is unknown")
if self._last_plan != None:
if (len(self._last_plan.joint_trajectory.points) == 0
and len(self._last_plan.multi_dof_joint_trajectory.
points) == 0):
self._last_plan = None
dest_str = clist[1]
if vals != None:
dest_str = "[" + " ".join([str(x) for x in vals]) + "]"
if self._last_plan != None:
return (MoveGroupInfoLevel.SUCCESS,
"Planned to " + dest_str)
else:
return (
MoveGroupInfoLevel.FAIL,
"Failed while planning to " + dest_str,
)
elif clist[0] == "pick":
self._last_plan = None
if g.pick(clist[1]):
return (MoveGroupInfoLevel.SUCCESS,
"Picked object " + clist[1])
else:
return (
MoveGroupInfoLevel.FAIL,
"Failed while trying to pick object " + clist[1],
)
elif clist[0] == "place":
self._last_plan = None
if g.place(clist[1]):
return (MoveGroupInfoLevel.SUCCESS,
"Placed object " + clist[1])
else:
return (
MoveGroupInfoLevel.FAIL,
"Failed while trying to place object " + clist[1],
)
elif clist[0] == "planner":
g.set_planner_id(clist[1])
return (MoveGroupInfoLevel.SUCCESS,
"Planner is now " + clist[1])
elif clist[0] == "record" or clist[0] == "rec":
g.remember_joint_values(clist[1])
return (
MoveGroupInfoLevel.SUCCESS,
"Remembered current joint values under the name " +
clist[1],
)
elif clist[0] == "rand" or clist[0] == "random":
g.remember_joint_values(clist[1], g.get_random_joint_values())
return (
MoveGroupInfoLevel.SUCCESS,
"Remembered random joint values under the name " +
clist[1],
)
elif clist[0] == "del" or clist[0] == "delete":
g.forget_joint_values(clist[1])
return (
MoveGroupInfoLevel.SUCCESS,
"Forgot joint values under the name " + clist[1],
)
elif clist[0] == "show":
known = g.get_remembered_joint_values()
if clist[1] in known:
return (
MoveGroupInfoLevel.INFO,
"[" + " ".join([str(x)
for x in known[clist[1]]]) + "]",
)
else:
return (
MoveGroupInfoLevel.WARN,
"Joint values for " + clist[1] + " are not known",
)
elif clist[0] == "tol" or clist[0] == "tolerance":
try:
g.set_goal_tolerance(float(clist[1]))
return (MoveGroupInfoLevel.SUCCESS, "OK")
except:
return (
MoveGroupInfoLevel.WARN,
"Unable to parse tolerance value '" + clist[1] + "'",
)
elif clist[0] == "time":
try:
g.set_planning_time(float(clist[1]))
return (MoveGroupInfoLevel.SUCCESS, "OK")
except:
return (
MoveGroupInfoLevel.WARN,
"Unable to parse planning duration value '" +
clist[1] + "'",
)
elif clist[0] == "constrain":
try:
g.set_path_constraints(clist[1])
return (MoveGroupInfoLevel.SUCCESS, "OK")
except:
if self._db_host != None:
return (
MoveGroupInfoLevel.WARN,
"Constraint " + clist[1] + " is not known.",
)
else:
return (MoveGroupInfoLevel.WARN,
"Not connected to a database.")
elif clist[0] == "wait":
try:
time.sleep(float(clist[1]))
return (MoveGroupInfoLevel.SUCCESS,
clist[1] + " seconds passed")
except:
return (
MoveGroupInfoLevel.WARN,
"Unable to wait '" + clist[1] + "' seconds",
)
elif clist[0] == "database":
try:
g.set_constraints_database(clist[1], self._db_port)
self._db_host = clist[1]
return (
MoveGroupInfoLevel.SUCCESS,
"Connected to " + self._db_host + ":" +
str(self._db_port),
)
except:
return (
MoveGroupInfoLevel.WARN,
"Unable to connect to '" + clist[1] + ":" +
str(self._db_port) + "'",
)
else:
return (MoveGroupInfoLevel.WARN,
"Unknown command: '" + cmd + "'")
if len(clist) == 3:
if clist[0] == "go" and clist[1] in self.GO_DIRS:
self._last_plan = None
try:
offset = float(clist[2])
(axis, factor) = self.GO_DIRS[clist[1]]
return self.command_go_offset(g, offset, factor, axis,
clist[1])
except MoveItCommanderException as e:
return (
MoveGroupInfoLevel.WARN,
"Going " + clist[1] + ": " + str(e),
)
except:
return (
MoveGroupInfoLevel.WARN,
"Unable to parse distance '" + clist[2] + "'",
)
elif clist[0] == "allow" and (clist[1] == "look"
or clist[1] == "looking"):
if (clist[2] == "1" or clist[2] == "true" or clist[2] == "T"
or clist[2] == "True"):
g.allow_looking(True)
else:
g.allow_looking(False)
return (MoveGroupInfoLevel.DEBUG, "OK")
elif clist[0] == "allow" and (clist[1] == "replan"
or clist[1] == "replanning"):
if (clist[2] == "1" or clist[2] == "true" or clist[2] == "T"
or clist[2] == "True"):
g.allow_replanning(True)
else:
g.allow_replanning(False)
return (MoveGroupInfoLevel.DEBUG, "OK")
elif clist[0] == "database":
try:
g.set_constraints_database(clist[1], int(clist[2]))
self._db_host = clist[1]
self._db_port = int(clist[2])
return (
MoveGroupInfoLevel.SUCCESS,
"Connected to " + self._db_host + ":" +
str(self._db_port),
)
except:
self._db_host = None
return (
MoveGroupInfoLevel.WARN,
"Unable to connect to '" + clist[1] + ":" + clist[2] +
"'",
)
if len(clist) == 4:
if clist[0] == "rotate":
try:
g.set_rpy_target([float(x) for x in clist[1:]])
if g.go():
return (MoveGroupInfoLevel.SUCCESS,
"Rotation complete")
else:
return (
MoveGroupInfoLevel.FAIL,
"Failed while rotating to " + " ".join(clist[1:]),
)
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.WARN, str(e))
except:
return (
MoveGroupInfoLevel.WARN,
"Unable to parse X-Y-Z rotation values '" +
" ".join(clist[1:]) + "'",
)
if len(clist) >= 7:
if clist[0] == "go":
self._last_plan = None
approx = False
if len(clist) > 7:
if clist[7] == "approx" or clist[7] == "approximate":
approx = True
try:
p = Pose()
p.position.x = float(clist[1])
p.position.y = float(clist[2])
p.position.z = float(clist[3])
q = tf.transformations.quaternion_from_euler(
float(clist[4]), float(clist[5]), float(clist[6]))
p.orientation.x = q[0]
p.orientation.y = q[1]
p.orientation.z = q[2]
p.orientation.w = q[3]
if approx:
g.set_joint_value_target(p, True)
else:
g.set_pose_target(p)
if g.go():
return (
MoveGroupInfoLevel.SUCCESS,
"Moved to pose target\n%s\n" % (str(p)),
)
else:
return (
MoveGroupInfoLevel.FAIL,
"Failed while moving to pose \n%s\n" % (str(p)),
)
except MoveItCommanderException as e:
return (
MoveGroupInfoLevel.WARN,
"Going to pose target: %s" % (str(e)),
)
except:
return (
MoveGroupInfoLevel.WARN,
"Unable to parse pose '" + " ".join(clist[1:]) + "'",
)
return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
def command_show(self, g):
known = g.get_remembered_joint_values()
res = []
for k in known.keys():
res.append(k + " = [" + " ".join([str(x) for x in known[k]]) + "]")
return (MoveGroupInfoLevel.INFO, "\n".join(res))
def command_current(self, g):
res = ("joints = [" +
", ".join([str(x) for x in g.get_current_joint_values()]) + "]")
if len(g.get_end_effector_link()) > 0:
res = (res + "\n\n" + g.get_end_effector_link() + " pose = [\n" +
str(g.get_current_pose()) + " ]")
res = (res + "\n" + g.get_end_effector_link() + " RPY = " +
str(g.get_current_rpy()))
return (MoveGroupInfoLevel.INFO, res)
def command_go_offset(self, g, offset, factor, dimension_index,
direction_name):
if g.has_end_effector_link():
try:
g.shift_pose_target(dimension_index, factor * offset)
if g.go():
return (
MoveGroupInfoLevel.SUCCESS,
"Moved " + direction_name + " by " + str(offset) +
" m",
)
else:
return (
MoveGroupInfoLevel.FAIL,
"Failed while moving " + direction_name,
)
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.WARN, str(e))
except:
return (MoveGroupInfoLevel.WARN,
"Unable to process pose update")
else:
return (
MoveGroupInfoLevel.WARN,
"No known end effector. Cannot move " + direction_name,
)
def resolve_command_alias(self, cmdorig):
cmd = cmdorig.lower()
if cmd == "which":
return "id"
if cmd == "groups":
return "use"
return cmdorig
def get_help(self):
res = []
res.append("Known commands:")
res.append(" help show this screen")
res.append(
" allow looking <true|false> enable/disable looking around")
res.append(
" allow replanning <true|false> enable/disable replanning")
res.append(" constrain clear path constraints")
res.append(
" constrain <name> use the constraint <name> as a path constraint"
)
res.append(
" current show the current state of the active group")
res.append(
" database display the current database connection (if any)"
)
res.append(
" delete <name> forget the joint values under the name <name>"
)
res.append(
" eef print the name of the end effector attached to the current group"
)
res.append(
" execute execute a previously computed motion plan")
res.append(
" go <name> plan and execute a motion to the state <name>"
)
res.append(
" go rand plan and execute a motion to a random state"
)
res.append(
" go <dir> <dx>| plan and execute a motion in direction up|down|left|right|forward|backward for distance <dx>"
)
res.append(
" ground add a ground plane to the planning scene")
res.append(
" id|which display the name of the group that is operated on"
)
res.append(
" joints display names of the joints in the active group"
)
res.append(
" load [<file>] load a set of interpreted commands from a file"
)
res.append(" pick <name> pick up object <name>")
res.append(" place <name> place object <name>")
res.append(" plan <name> plan a motion to the state <name>")
res.append(" plan rand plan a motion to a random state")
res.append(
" planner <name> use planner <name> to plan next motion")
res.append(
" record <name> record the current joint values under the name <name>"
)
res.append(
" rotate <x> <y> <z> plan and execute a motion to a specified orientation (about the X,Y,Z axes)"
)
res.append(
" save [<file>] save the currently known variables as a set of commands"
)
res.append(
" show display the names and values of the known states"
)
res.append(" show <name> display the value of a state")
res.append(" stop stop the active group")
res.append(
" time show the configured allowed planning time")
res.append(" time <val> set the allowed planning time")
res.append(
" tolerance show the tolerance for reaching the goal region"
)
res.append(
" tolerance <val> set the tolerance for reaching the goal region"
)
res.append(
" trace <on|off> enable/disable replanning or looking around"
)
res.append(
" use <name> switch to using the group named <name> (and load it if necessary)"
)
res.append(
" use|groups show the group names that are already loaded"
)
res.append(
" vars display the names of the known states")
res.append(" wait <dt> sleep for <dt> seconds")
res.append(" x = y assign the value of y to x")
res.append(" x = [v1 v2...] assign a vector of values to x")
res.append(
" x[idx] = val assign a value to dimension idx of x")
return "\n".join(res)
def get_keywords(self):
known_vars = []
known_constr = []
if self.get_active_group() != None:
known_vars = self.get_active_group().get_remembered_joint_values(
).keys()
known_constr = self.get_active_group().get_known_constraints()
groups = self._robot.get_group_names()
return {
"go":
["up", "down", "left", "right", "backward", "forward", "random"] +
list(known_vars),
"help": [],
"record":
known_vars,
"show":
known_vars,
"wait": [],
"delete":
known_vars,
"database": [],
"current": [],
"use":
groups,
"load": [],
"save": [],
"pick": [],
"place": [],
"plan":
known_vars,
"allow": ["replanning", "looking"],
"constrain":
known_constr,
"vars": [],
"joints": [],
"tolerance": [],
"time": [],
"eef": [],
"execute": [],
"ground": [],
"id": [],
}
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Construct the initial scene object
scene = PlanningSceneInterface()
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# Create a dictionary to hold object colors
self.colors = dict()
# Pause for the scene to get ready
rospy.sleep(1)
# Initialize the move group
both_arms = moveit_commander.MoveGroupCommander('both_arms')
right_arm = moveit_commander.MoveGroupCommander('right_arm')
left_arm = moveit_commander.MoveGroupCommander('left_arm')
left_gripper = MoveGroupCommander('left_gripper')
# Get the name of the end-effector link
right_eef = right_arm.get_end_effector_link()
left_eef = left_arm.get_end_effector_link()
print "============ Printing end-effector link"
print right_eef
print left_eef
# Set the reference frame for pose targets
right_reference_frame = right_arm.get_planning_frame()
left_reference_frame = left_arm.get_planning_frame()
# Set the right arm reference frame accordingly
#right_arm.set_pose_reference_frame(reference_frame)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.01)
right_arm.set_goal_orientation_tolerance(0.01)
left_arm.allow_replanning(True)
# Allow some leeway in position (meters) and orientation (radians)
left_arm.set_goal_position_tolerance(0.01)
left_arm.set_goal_orientation_tolerance(0.01)
# Allow 5 seconds per planning attempt
left_arm.set_planning_time(5)
right_arm.set_planning_time(5)
object1_id = 'object1'
# Remove leftover objects from a previous run
scene.remove_world_object(object1_id)
rospy.sleep(2)
right_arm.set_named_target('right_ready')
right_arm.go()
left_arm.set_named_target('left_ready')
left_arm.go()
left_gripper.set_joint_value_target(GRIPPER_OPEN)
left_gripper.go()
rospy.sleep(1)
# attach_box
# Set the length, width and height of the object to attach
object1_size = [0.09, 0.057, 0.001]
# Create a pose for the tool relative to the end-effector
object1_p = PoseStamped()
object1_p.header.frame_id = right_reference_frame
object1_p.pose.position.x = 0.77
object1_p.pose.position.y = -0.27
object1_p.pose.position.z = -0.21
object1_p.pose.orientation.w = 1
scene.add_box(object1_id, object1_p, object1_size)
self.setColor(object1_id, 0.8, 0.4, 0, 1.0)
self.sendColors()
right_arm.set_start_state_to_current_state()
left_arm.set_start_state_to_current_state()
# target pose
target_pose1 = PoseStamped()
target_pose1.header.frame_id = right_reference_frame
target_pose1.header.stamp = rospy.Time.now()
target_pose1.pose.position.x = 0.77
target_pose1.pose.position.y = -0.27
target_pose1.pose.position.z = -0.14
target_pose1.pose.orientation.x = 0.500166303975
target_pose1.pose.orientation.y = 0.865326245156
target_pose1.pose.orientation.z = -0.0155702691449
target_pose1.pose.orientation.w = 0.0283147405248
right_arm.set_pose_target(target_pose1, right_eef)
right_arm.go()
rospy.sleep(1)
# Attach the tool to the end-effector
scene.attach_box(right_eef, object1_id, object1_p, object1_size)
target_pose2 = PoseStamped()
target_pose2.header.frame_id = right_reference_frame
target_pose2.header.stamp = rospy.Time.now()
target_pose2.pose.position.x = 0.632
target_pose2.pose.position.y = -0.125
target_pose2.pose.position.z = 0.08
target_pose2.pose.orientation.x = 0.2392
target_pose2.pose.orientation.y = -0.9708
target_pose2.pose.orientation.z = -0.0137
target_pose2.pose.orientation.w = 0.0103
target_pose3 = PoseStamped()
target_pose3.header.frame_id = left_reference_frame
target_pose3.header.stamp = rospy.Time.now()
target_pose3.pose.position.x = 0.632
target_pose3.pose.position.y = 0.026
target_pose3.pose.position.z = 0.013
target_pose3.pose.orientation.x = -0.021
target_pose3.pose.orientation.y = 0.017
target_pose3.pose.orientation.z = -0.999
target_pose3.pose.orientation.w = 0.031
both_arms.set_pose_target(target_pose2, right_eef)
both_arms.set_pose_target(target_pose3, left_eef)
both_arms.go()
rospy.sleep(3)
#scene.remove_attached_object(right_eef, 'tool')
object1_p = left_arm.get_current_pose(left_eef)
object1_p.pose.position.y -= 0.12
object1_p.pose.orientation.x = 0
object1_p.pose.orientation.y = 0
object1_p.pose.orientation.z = 0.707
object1_p.pose.orientation.w = 0.707
scene.remove_attached_object(right_eef, object1_id)
scene.attach_box(left_eef, object1_id, object1_p, object1_size)
rospy.sleep(1)
left_gripper.set_joint_value_target(GRIPPER_CLOSE)
left_gripper.go()
target_pose4 = PoseStamped()
target_pose4.header.frame_id = right_reference_frame
target_pose4.header.stamp = rospy.Time.now()
target_pose4.pose.position.x = 0.58241
target_pose4.pose.position.y = -0.30286
target_pose4.pose.position.z = 0.05471
target_pose4.pose.orientation.x = 0.23931
target_pose4.pose.orientation.y = -0.97080
target_pose4.pose.orientation.z = -0.01346
target_pose4.pose.orientation.w = 0.01003
target_pose5 = PoseStamped()
target_pose5.header.frame_id = left_reference_frame
target_pose5.header.stamp = rospy.Time.now()
target_pose5.pose.position.x = 0.843
target_pose5.pose.position.y = 0.438
target_pose5.pose.position.z = 0.105
target_pose5.pose.orientation.x = -0.01386
target_pose5.pose.orientation.y = -0.02950
target_pose5.pose.orientation.z = -0.70489
target_pose5.pose.orientation.w = 0.70856
#right_arm.set_pose_target(target_pose4, right_eef)
both_arms.set_pose_target(target_pose4, right_eef)
both_arms.set_pose_target(target_pose5, left_eef)
both_arms.go()
rospy.sleep(2)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
