class PickPlaceDemo():
def __init__(self):
self._scene = PlanningSceneInterface('base_link')
self._pickplace = PickPlaceInterface('xarm5', 'gripper', verbose=True)
self._move_group = MoveGroupInterface('xarm5', 'base_link')
def setup_scene(self):
# remove previous objects
for name in self._scene.getKnownCollisionObjects():
self._scene.removeCollisionObject(name, False)
for name in self._scene.getKnownAttachedObjects():
self._scene.removeAttachedObject(name, False)
self._scene.waitForSync()
self.__addBoxWithOrientation('box',
0.25,
0.25,
0.25,
-0.45,
0.1,
0.125,
0,
0,
np.radians(45),
wait=False)
self._scene.waitForSync()
def __addBoxWithOrientation(self,
name,
size_x,
size_y,
size_z,
x,
y,
z,
roll,
pitch,
yaw,
wait=True):
s = SolidPrimitive()
s.dimensions = [size_x, size_y, size_z]
s.type = s.BOX
ps = PoseStamped()
ps.header.frame_id = self._scene._fixed_frame
ps.pose.position.x = x
ps.pose.position.y = y
ps.pose.position.z = z
q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
ps.pose.orientation.x = q[0]
ps.pose.orientation.y = q[1]
ps.pose.orientation.z = q[2]
ps.pose.orientation.w = q[3]
self._scene.addSolidPrimitive(name, s, ps.pose, wait)
def pickup(self):
g = Grasp()
self._pickplace.pickup("box", [g], support_name="box")
def setup_scene():
scene = PlanningSceneInterface('base_link')
scene.removeCollisionObject('box')
scene.addCube('box', 0.25, -0.45, 0.1, 0.125)
pick_place = PickPlaceInterface('xarm5', 'gripper', verbose=True)
grasp = Grasp()
# fill in g
# setup object named object_name using PlanningSceneInterface
pick_place.pickup('box', [grasp], support_name='supporting_surface')
place_loc = PlaceLocation()
# fill in l
pick_place.place('box'[place_loc],
goal_is_eef=True,
support_name='supporting_surface')
