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")
class GraspingClient(object):
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.move_group = MoveGroupInterface("arm", "base_link")
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
def updateScene(self):
# find objects
rospy.loginfo("Add collision objects")
self.scene.removeCollisionObject("my_front_ground")
self.scene.removeCollisionObject("my_back_ground")
self.scene.removeCollisionObject("my_right_ground")
self.scene.removeCollisionObject("my_left_ground")
self.scene.removeCollisionObject("box_1")
self.scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
self.scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
self.scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
self.scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
self.scene.addBox("box_1", 0.44, 0.43, 0.68, 0.7, -0.4, .34)
self.scene.waitForSync()
def tuck(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def zero(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
class GripperInterfaceClient():
def __init__(self):
self.joint_names = ["l_gripper_finger_joint", "r_gripper_finger_joint"]
self.client = actionlib.SimpleActionClient("gripper_controller/gripper_action",
GripperCommandAction)
rospy.loginfo("Waiting for gripper_controller...")
self.client.wait_for_server()
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True, plan_only=True)
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
def execute_trajectory(self, trajectory):
goal_position = trajectory.points[-1].positions
print '==========', goal_position
command_goal = GripperCommandGoal()
command_goal.command.position = goal_position[0]+goal_position[1]
command_goal.command.max_effort = 50.0 # Placeholder. TODO Figure out a better way to compute this
self.client.send_goal(command_goal)
self.client.wait_for_result()
def move_to(self, goal_position, max_effort):
command_goal = GripperCommandGoal()
command_goal.command.position = goal_position
command_goal.command.max_effort = max_effort
self.client.send_goal(command_goal)
self.client.wait_for_result()
def updateScene(self):
# find objects
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
find_result = self.find_client.get_result()
# 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()
# insert objects to scene
idx = -1
for obj in find_result.objects:
idx += 1
obj.object.name = "object%d"%idx
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
wait = False)
# Try mesh representation
#idx = -1
#for obj in find_result.objects:
# idx += 1
# obj.object.name = "object%d"%idx
# self.scene.addMesh(obj.object.name,
# obj.object.mesh_poses[0],
# obj.object.meshes[0],
# wait = False)
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0] + 0.1,
2.5, # wider
obj.primitives[0].dimensions[2] + height]
obj.primitive_poses[0].position.z += -height/2.0
# add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
wait = False)
self.scene.waitForSync()
# store for grasping
self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
def getGraspableCube(self):
graspable = None
for obj in self.objects:
# need grasps
if len(obj.grasps) < 1:
continue
# check size
# if obj.object.primitives[0].dimensions[0] < 0.03 or \
# obj.object.primitives[0].dimensions[0] > 0.04 or \
# obj.object.primitives[0].dimensions[0] < 0.07 or \
# obj.object.primitives[0].dimensions[0] > 0.09 or \
# obj.object.primitives[2].dimensions[2] < 0.19 or \
# obj.object.primitives[2].dimensions[2] > 0.20:
# continue
# has to be on table
#if obj.object.primitive_poses[0].position.z < 0.5:
# continue
return obj.object, obj.grasps
# nothing detected
return None, None
def plan_pick(self, block, grasps):
success, pick_result = self.pickplace.pick_with_retry(block.name,
grasps,
support_name=block.support_surface,
allow_gripper_support_collision=False,
planner_id = 'PRMkConfigDefault',
retries = 2,
scene=self.scene)
#self.pick_result = pick_result
if success:
return pick_result.trajectory_stages
else:
rospy.loginfo("Planning Failed.")
return None
class GraspingClient(object):
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.move_group = MoveGroupInterface("arm", "base_link")
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(
find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
def updateScene(self):
# find objects
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
find_result = self.find_client.get_result()
# 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()
# insert objects to scene
idx = -1
for obj in find_result.objects:
idx += 1
obj.object.name = "object%d" % idx
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
wait=False)
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [
obj.primitives[0].dimensions[0],
1.5, # wider
obj.primitives[0].dimensions[2] + height
]
obj.primitive_poses[0].position.z += -height / 2.0
# add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
wait=False)
self.scene.waitForSync()
# store for grasping
self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
def getGraspableCube(self):
graspable = None
for obj in self.objects:
# need grasps
if len(obj.grasps) < 1:
continue
# check size
if obj.object.primitives[0].dimensions[0] < 0.05 or \
obj.object.primitives[0].dimensions[0] > 0.07 or \
obj.object.primitives[0].dimensions[0] < 0.05 or \
obj.object.primitives[0].dimensions[0] > 0.07 or \
obj.object.primitives[0].dimensions[0] < 0.05 or \
obj.object.primitives[0].dimensions[0] > 0.07:
continue
# has to be on table
if obj.object.primitive_poses[0].position.z < 0.5:
continue
return obj.object, obj.grasps
# nothing detected
return None, None
def getSupportSurface(self, name):
for surface in self.support_surfaces:
if surface.name == name:
return surface
return None
def getPlaceLocation(self):
pass
def pick(self, block, grasps):
success, pick_result = self.pickplace.pick_with_retry(
block.name,
grasps,
support_name=block.support_surface,
scene=self.scene)
self.pick_result = pick_result
return success
def place(self, block, pose_stamped):
places = list()
l = PlaceLocation()
l.place_pose.pose = pose_stamped.pose
l.place_pose.header.frame_id = pose_stamped.header.frame_id
# copy the postrection
m = 16 # number of possible place poses
pi = 3.141592653589
for i in range(0, m - 1):
l.place_pose.pose = rotate_pose_msg_by_euler_angles(
l.place_pose.pose, 0, 0, 2 * pi / m)
places.append(copy.deepcopy(l))
success, place_result = self.pickplace.place_with_retry(
block.name, places, scene=self.scene)
return success
def tuck(self):
joints = [
"shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
class GraspingClient(object):
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.move_group = MoveGroupInterface("arm", "base_link")
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
def updateScene(self):
# find objects
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
find_result = self.find_client.get_result()
rospy.loginfo("Found %d objects" % len(find_result.objects))
# 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()
# insert objects to scene
objects = list()
idx = -1
for obj in find_result.objects:
idx += 1
obj.object.name = "obj%d"%idx
self.scene.addSolidPrimitive(obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], use_service=False)
if obj.object.primitive_poses[0].position.x < 1.3: #< 0.85:
objects.append([obj, obj.object.primitive_poses[0].position.y])
#for obj in find_result.support_surfaces:
# # extend surface to floor, and make wider since we have narrow field of view
# height = obj.primitive_poses[0].position.z
# obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0], 1.5, # wider
# obj.primitives[0].dimensions[2] + height]
# obj.primitive_poses[0].position.z += -height/2.0
# # add to scene
# self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0])
self.scene.waitForSync()
# store for grasping
self.surfaces = find_result.support_surfaces
# store graspable objects by y
objects.sort(key=lambda object: object[1])
#objects.reverse()
self.objects = [object[0] for object in objects]
def getGraspableObject(self, goal_obj_x, goal_obj_y):
graspable = None
for obj in self.objects:
# need grasps
if len(obj.grasps) < 1:
continue
# check size
if obj.object.primitives[0].dimensions[0] < 0.03 or \
obj.object.primitives[0].dimensions[0] > 0.25 or \
obj.object.primitives[0].dimensions[0] < 0.03 or \
obj.object.primitives[0].dimensions[0] > 0.25 or \
obj.object.primitives[0].dimensions[0] < 0.03 or \
obj.object.primitives[0].dimensions[0] > 0.25:
continue
# has to be on table
if obj.object.primitive_poses[0].position.z < 0.4:
continue
if obj.object.primitive_poses[0].position.x < 0.3:
continue
# get goal object
if ( abs(obj.object.primitive_poses[0].position.x - goal_obj_x) > 0.03):
continue
if ( abs(obj.object.primitive_poses[0].position.y - goal_obj_y) > 0.03):
continue
print("**************************************Object Name: ", obj.object.name)
print("Object Pose: ", obj.object.primitive_poses[0], obj.object.primitives[0])
return obj.object, obj.grasps
# nothing detected
return None, None
def getSupportSurface(self, name):
for surface in self.support_surfaces:
if surface.name == name:
return surface
return None
def getPlaceLocation(self):
pass
def pick(self, block, grasps):
success, pick_result = self.pickplace.pick_with_retry(block.name, grasps, support_name=block.support_surface, scene=self.scene)
self.pick_result = pick_result
#hold the object
#touch_links = ['l_gripper_finger_link', 'r_gripper_finger_link']
#self.hand_group.attach_object('apple', 'base_link', touch_links=touch_links)
return success
def place(self, block, pose_stamped):
places = list()
l = PlaceLocation()
l.place_pose.pose = pose_stamped.pose
l.place_pose.header.frame_id = pose_stamped.header.frame_id
# copy the posture, approach and retreat from the grasp used
l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach
l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
places.append(copy.deepcopy(l))
# create another several places, rotate each by 360/m degrees in yaw direction
m = 16 # number of possible place poses
pi = 3.141592653589
for i in range(0, m-1):
#l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 2 * pi / m)
places.append(copy.deepcopy(l))
success, place_result = self.pickplace.place_with_retry(block.name, places, scene=self.scene)
return success
def tuck(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def stow(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
#pose = [-1.60, -1.10, -1.20, -1.50, 0.0, -1.51, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def intermediate_stow(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [0.7, -0.3, 0.0, -0.3, 0.0, -0.57, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def picknplace():
# Define initial parameters
p = PlanningSceneInterface("base")
g = MoveGroupInterface("both_arms", "base")
gr = MoveGroupInterface("right_arm", "base")
gl = MoveGroupInterface("left_arm", "base")
leftgripper = baxter_interface.Gripper('left')
leftgripper.calibrate()
leftgripper.open()
jts_both = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2', 'right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
jts_right = ['right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
jts_left = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2']
pos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519,1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719]
pos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384, 1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323]
lpos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519]
lpos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384]
rpos1 = [1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719]
rpos2 = [1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323]
# Clear planning scene
p.clear()
# Add table as attached object
p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal'])
# Move both arms to start state
g.moveToJointPosition(jts_both, pos1, plan_only=False)
# Get obj locations
temp = rospy.wait_for_message("Dpos", PoseArray)
locs = temp.poses
locs_x = []
locs_y = []
orien = []
size = []
for i in range(len(locs)):
locs_x.append(0.57 + locs[i].position.x)
locs_y.append(-0.011 + locs[i].position.y)
orien.append(locs[i].position.z*pi/180)
size.append(locs[i].orientation.x)
# Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene
ind_rmv = []
for i in range(0,len(locs)):
if (locs_y[i] > 0.42):
ind_rmv.append(i)
continue
for j in range(i,len(locs)):
if not (i == j):
if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018:
ind_rmv.append(i)
locs_x = del_meth(locs_x, ind_rmv)
locs_y = del_meth(locs_y, ind_rmv)
orien = del_meth(orien, ind_rmv)
size = del_meth(size, ind_rmv)
# Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes
if locs_x:
ig0 = itemgetter(0)
sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0))
locs_x = list(sorted_lists[1])
locs_y = list(sorted_lists[2])
orien = list(sorted_lists[3])
size = list(sorted_lists[0])
# Loop to continue pick and place until all objects are cleared from table
j=0
k=0
while locs_x:
p.clear() # CLear planning scene of all collision objects
# Attach table as attached collision object to base of baxter
p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal'])
xn = locs_x[0]
yn = locs_y[0]
zn = -0.06
thn = orien[0]
sz = size[0]
if thn > pi/4:
thn = -1*(thn%(pi/4))
# Add collision objects into planning scene
objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11']
for i in range(1,len(locs_x)):
p.addCube(objlist[i], 0.05, locs_x[i], locs_y[i], -0.05)
p.waitForSync()
# Move both arms to pos2 (Right arm away and left arm on table)
g.moveToJointPosition(jts_both, pos2, plan_only=False)
# Move left arm to pick object and pick object
pickgoal = PoseStamped()
pickgoal.header.frame_id = "base"
pickgoal.header.stamp = rospy.Time.now()
pickgoal.pose.position.x = xn
pickgoal.pose.position.y = yn
pickgoal.pose.position.z = zn+0.1
pickgoal.pose.orientation.x = 1.0
pickgoal.pose.orientation.y = 0.0
pickgoal.pose.orientation.z = 0.0
pickgoal.pose.orientation.w = 0.0
gl.moveToPose(pickgoal, "left_gripper", plan_only=False)
# Orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles
pickgoal.pose = rotate_pose_msg_by_euler_angles(pickgoal.pose, 0.0, 0.0, thn)
gl.moveToPose(pickgoal, "left_gripper", plan_only=False)
pickgoal.pose.position.z = zn
gl.moveToPose(pickgoal, "left_gripper", max_velocity_scaling_factor = 0.14, plan_only=False)
leftgripper.close()
pickgoal.pose.position.z = zn+0.1
gl.moveToPose(pickgoal, "left_gripper", plan_only=False)
# Move both arms to pos1
g.moveToJointPosition(jts_both, pos1, plan_only=False)
# Get obj locations
temp = rospy.wait_for_message("Dpos", PoseArray)
locs = temp.poses
locs_x = []
locs_y = []
orien = []
size = []
for i in range(len(locs)):
locs_x.append(0.57 + locs[i].position.x)
locs_y.append(-0.011 + locs[i].position.y)
orien.append(locs[i].position.z*pi/180)
size.append(locs[i].orientation.x)
# Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene
ind_rmv = []
for i in range(0,len(locs)):
if (locs_y[i] > 0.42):
ind_rmv.append(i)
continue
for j in range(i,len(locs)):
if not (i == j):
if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018:
ind_rmv.append(i)
locs_x = del_meth(locs_x, ind_rmv)
locs_y = del_meth(locs_y, ind_rmv)
orien = del_meth(orien, ind_rmv)
size = del_meth(size, ind_rmv)
# Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes
if locs_x:
ig0 = itemgetter(0)
sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0))
locs_x = list(sorted_lists[1])
locs_y = list(sorted_lists[2])
orien = list(sorted_lists[3])
size = list(sorted_lists[0])
# Place object
stleft = PoseStamped()
stleft.header.frame_id = "base"
stleft.header.stamp = rospy.Time.now()
stleft.pose.position.x = 0.65
stleft.pose.position.y = 0.55
stleft.pose.position.z = 0.1
stleft.pose.orientation.x = 1.0
stleft.pose.orientation.y = 0.0
stleft.pose.orientation.z = 0.0
stleft.pose.orientation.w = 0.0
# Stack objects (large cubes) if size if greater than some threshold
if sz > 16.:
stleft.pose.position.x = 0.65
stleft.pose.position.y = 0.7
stleft.pose.position.z = -0.04+(k*0.05)
k += 1
gl.moveToPose(stleft, "left_gripper", plan_only=False)
leftgripper.open()
stleft.pose.position.z = 0.3
gl.moveToPose(stleft, "left_gripper", plan_only=False)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
ptu_action = FollowTrajectoryClient(
"ptu_controller", ["ptu_pan_joint", "ptu_tilt_joint"])
ptu_action.move_to([
0.0,
-0.75,
])
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface(REFERENCE_FRAME)
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)
# 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()
print("End Effector Link", end_effector_link)
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.02)
right_arm.set_goal_orientation_tolerance(0.05)
# 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 15 seconds per planning attempt
right_arm.set_planning_time(15)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 10
# Set a limit on the number of place attempts
max_place_attempts = 3
# Give the scene a chance to catch up
rospy.sleep(2)
# Connect to the UBR-1 find_objects action server
find_topic = "basic_grasping_perception/find_objects"
sub_topic = "handles_position"
rospy.Subscriber(sub_topic, target_pose, getHandlesCb)
rospy.loginfo(
"Connecting to basic_grasping_perception/find_objects...")
find_objects = actionlib.SimpleActionClient(
find_topic, FindGraspableObjectsAction)
find_objects.wait_for_server()
rospy.loginfo("...connected")
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
# right_arm.set_named_target('right_down')
# right_arm.go()
# Open the gripper to the neutral position
# right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
# right_gripper.go()
# rospy.sleep(1)
# Begin the main perception and pick-and-place loop
while not rospy.is_shutdown():
# Initialize the grasping goal
goal = FindGraspableObjectsGoal()
# We don't use the UBR-1 grasp planner as it does not work with our gripper
goal.plan_grasps = False
# Send the goal request to the find_objects action server which will trigger
# the perception pipeline
find_objects.send_goal(goal)
# Wait for a result
find_objects.wait_for_result(rospy.Duration(10.0))
# The result will contain support surface(s) and objects(s) if any are detected
find_result = find_objects.get_result()
# Display the number of objects found
rospy.loginfo("Found %d objects" % len(find_result.objects))
rospy.loginfo("Found %d Support Surfaces" %
len(find_result.support_surfaces))
# print("perception_results_primitives",find_result.objects.primitives[0])
# print("perception_results_primitives_poses",find_result.objects.primitives_poses)
# Remove all previous objects from the planning scene
# for name in scene.getKnownCollisionObjects():
# scene.removeCollisionObject(name, False)
# for name in scene.getKnownAttachedObjects():
# scene.removeAttachedObject(name, False)
# scene.waitForSync()
# Clear the virtual object colors
scene._colors = dict()
# Use the nearest object on the table as the target
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_size = None
# target_position = None
the_object = None
the_object_dist = 1.0
count = -1
# target_size
# Cycle through all detected objects and keep the nearest one
for obj in find_result.objects:
count += 1
scene.addSolidPrimitive(
"object%d" % count,
obj.object.primitives[0],
obj.object.primitive_poses[0],
) #wait = False
# Choose the object nearest to the robot
dx = obj.object.primitive_poses[0].position.x - args.x
dy = obj.object.primitive_poses[0].position.y
d = math.sqrt((dx * dx) + (dy * dy))
if d < the_object_dist:
the_object_dist = d
the_object = count
# Get the size of the target
target_size = obj.object.primitives[0].dimensions
print("target_size %d ", count, target_size)
# target_position = obj.object.primitive_poses[0].position
# print("target_size %d ", %count, target_size )
# Set the target pose
target_pose.pose = obj.object.primitive_poses[0]
print("target_pose_before ", target_pose)
# We want the gripper to be horizontal
target_pose.pose.orientation.x = 0.519617092464
target_pose.pose.orientation.y = -0.510439243162
target_pose.pose.orientation.z = 0.479912175114
target_pose.pose.orientation.w = -0.489013456294
print("target_pose_after ", target_pose)
# Make sure we found at least one object before setting the target ID
if the_object != None:
target_id = "object%d" % the_object
# Insert the support surface into the planning scene
for obj in find_result.support_surfaces:
# Extend surface to floor
# height = obj.primitive_poses[0].position.z
# obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0],
# 2.0, # make table wider
# obj.primitives[0].dimensions[2] ]
# obj.primitive_poses[0].position.z += -height/1.5
# Add to scene
scene.addSolidPrimitive(
obj.name,
obj.primitives[0],
obj.primitive_poses[0],
) #wait = False
# Get the table dimensions
table_size = obj.primitives[0].dimensions
print("table_size", table_size)
# If no objects detected, try again
if the_object == None or target_size is None:
rospy.logerr("Nothing to grasp! try again...")
continue
# Wait for the scene to sync
scene.waitForSync()
# Set colors of the table and the object we are grabbing
scene.setColor(target_id, 223.0 / 256.0, 90.0 / 256.0,
12.0 / 256.0) # orange
scene.setColor(
find_result.objects[the_object].object.support_surface, 0.3,
0.3, 0.3, 0.7) # grey
scene.sendColors()
# Skip pick-and-place if we are just detecting objects
if args.objects:
if args.once:
exit(0)
else:
continue
# Get the support surface ID
#support_surface = find_result.objects[the_object].object.support_surface
# Set the support surface name to the table object
#right_arm.set_support_surface_name(support_surface)
# 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 = target_pose.pose.position.x
# place_pose.pose.position.y = 0.03
# place_pose.pose.position.z = table_size[2] + target_size[2] / 2.0 + 0.015
# place_pose.pose.orientation.w = 1.0
# Initialize the grasp pose to the target pose
grasp_pose = target_pose
# Shift the grasp pose half the size of the target to center it in the gripper
try:
grasp_pose.pose.position.x += target_size[0]
grasp_pose.pose.position.y -= 0.01
grasp_pose.pose.position.z += target_size[2] / 2.0
except:
rospy.loginfo("Invalid object size so skipping")
continue
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id])
# Publish the grasp poses so they can be viewed in RViz
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# Track success/failure and number of attempts for pick operation
result = None
n_attempts = 0
# Set the start state to the current state
right_arm.set_start_state_to_current_state()
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
result = right_arm.pick(target_id, grasps)
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
rospy.sleep(1.0)
# If the pick was successful, attempt the place operation
# if result == MoveItErrorCodes.SUCCESS:
# result = None
# n_attempts = 0
# # Generate valid place poses
# places = self.make_places(place_pose)
# # Set the start state to the current state
# #right_arm.set_start_state_to_current_state()
# # Repeat until we succeed or run out of attempts
# while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
# for place in places:
# result = right_arm.place(target_id, place)
# if result == MoveItErrorCodes.SUCCESS:
# break
# n_attempts += 1
# rospy.loginfo("Place attempt: " + str(n_attempts))
# rospy.sleep(0.2)
# if result != MoveItErrorCodes.SUCCESS:
# rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.")
else:
rospy.loginfo("Pick operation failed after " +
str(n_attempts) + " attempts.")
rospy.sleep(2)
# Open the gripper to the neutral position
# right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
# right_gripper.go()
# rospy.sleep(2)
# Return the arm to the "resting" pose stored in the SRDF file
# right_arm.set_named_target('resting')
# right_arm.go()
# rospy.sleep(2)
# Give the servos a rest
# arbotix_relax_all_servos()
# rospy.sleep(2)
if args.once:
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
class GraspingClient(object):
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.move_group = MoveGroupInterface("arm", "base_link")
grasp_topic = "fetch_grasp_planner_node/plan"
rospy.loginfo("Waiting for %s..." % grasp_topic)
self.grasp_planner_client = actionlib.SimpleActionClient(
grasp_topic, GraspPlanningAction)
wait = self.grasp_planner_client.wait_for_server(rospy.Duration(5))
if (wait):
print("successfully connected to grasp server")
else:
print("failed to connect to grasp server")
rospy.Subscriber("fetch_fruit_harvest/apple_pose", Pose,
self.apple_pose_callback)
self.pub = rospy.Publisher("fetch_fruit_harvest/grasp_msg",
String,
queue_size=10)
self.object = Object()
self.grasps = Grasp()
self.ready_for_grasp = False
self.pick_place_finished = False
self.first_time_grasp = True
self.scene.addBox("ground", 300, 300, 0.02, 0, 0, 0)
self.tuck()
def apple_pose_callback(self, message):
if self.pick_place_finished or self.first_time_grasp:
self.first_time_grasp = False
self.pick_place_finished = False
print("apple_pose_callback")
apple = SolidPrimitive()
apple.type = SolidPrimitive.SPHERE
apple.dimensions = [0.03, 0.03, 0.03]
self.object.name = "apple"
self.object.primitives = [apple]
self.object.primitive_poses = [message]
# add stamp and frame
self.object.header.stamp = rospy.Time.now()
self.object.header.frame_id = "base_link"
goal = GraspPlanningGoal()
goal.object = self.object
self.grasp_planner_client.send_goal(goal)
self.grasp_planner_client.wait_for_result(rospy.Duration(5.0))
grasp_planner_result = self.grasp_planner_client.get_result()
self.grasps = grasp_planner_result.grasps
self.ready_for_grasp = True
def updateScene(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()
# insert objects to scene
self.scene.addBox("ground", 300, 300, 0.02, 0, 0, 0)
self.scene.addSolidPrimitive(self.object.name,
self.object.primitives[0],
self.object.primitive_poses[0])
self.scene.waitForSync()
def getPlaceLocation(self):
pass
def pick(self, block, grasps):
success, pick_result = self.pickplace.pick_with_retry(block.name,
grasps,
scene=self.scene)
self.pick_result = pick_result
return success
def place(self, block, pose_stamped):
places = list()
l = PlaceLocation()
l.place_pose.pose = pose_stamped.pose
l.place_pose.header.frame_id = pose_stamped.header.frame_id
# copy the posture, approach and retreat from the grasp used
l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach
l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
places.append(copy.deepcopy(l))
# create another several places, rotate each by 360/m degrees in yaw direction
m = 16 # number of possible place poses
pi = 3.141592653589
for i in range(0, m - 1):
l.place_pose.pose = rotate_pose_msg_by_euler_angles(
l.place_pose.pose, 0, 0, 2 * pi / m)
places.append(copy.deepcopy(l))
success, place_result = self.pickplace.place_with_retry(
block.name, places, scene=self.scene)
return success
def tuck(self):
joints = [
"shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def stow(self):
joints = [
"shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
pose = [1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def intermediate_stow(self):
joints = [
"shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
pose = [0.7, -0.3, 0.0, -0.3, 0.0, -0.57, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
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("base_link")
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)
# 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(15)
# 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 = 3
# Give the scene a chance to catch up
rospy.sleep(2)
# Connect to the UBR-1 find_objects action server
rospy.loginfo("Connecting to basic_grasping_perception/find_objects...")
find_objects = actionlib.SimpleActionClient("basic_grasping_perception/find_objects", FindGraspableObjectsAction)
find_objects.wait_for_server()
rospy.loginfo("...connected")
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('resting')
right_arm.go()
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Begin the main perception and pick-and-place loop
while not rospy.is_shutdown():
# Initialize the grasping goal
goal = FindGraspableObjectsGoal()
# We don't use the UBR-1 grasp planner as it does not work with our gripper
goal.plan_grasps = False
# Send the goal request to the find_objects action server which will trigger
# the perception pipeline
find_objects.send_goal(goal)
# Wait for a result
find_objects.wait_for_result(rospy.Duration(5.0))
# The result will contain support surface(s) and objects(s) if any are detected
find_result = find_objects.get_result()
# Display the number of objects found
rospy.loginfo("Found %d objects" % len(find_result.objects))
# Remove all previous objects from the planning scene
for name in scene.getKnownCollisionObjects():
scene.removeCollisionObject(name, False)
for name in scene.getKnownAttachedObjects():
scene.removeAttachedObject(name, False)
scene.waitForSync()
# Clear the virtual object colors
scene._colors = dict()
# Use the nearest object on the table as the target
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_size = None
the_object = None
the_object_dist = 1.0
count = -1
# Cycle through all detected objects and keep the nearest one
for obj in find_result.objects:
count += 1
scene.addSolidPrimitive("object%d"%count,
obj.object.primitives[0],
obj.object.primitive_poses[0],
wait = False)
# Choose the object nearest to the robot
dx = obj.object.primitive_poses[0].position.x - args.x
dy = obj.object.primitive_poses[0].position.y
d = math.sqrt((dx * dx) + (dy * dy))
if d < the_object_dist:
the_object_dist = d
the_object = count
# Get the size of the target
target_size = obj.object.primitives[0].dimensions
# Set the target pose
target_pose.pose = obj.object.primitive_poses[0]
# We want the gripper to be horizontal
target_pose.pose.orientation.x = 0.0
target_pose.pose.orientation.y = 0.0
target_pose.pose.orientation.z = 0.0
target_pose.pose.orientation.w = 1.0
# Make sure we found at least one object before setting the target ID
if the_object != None:
target_id = "object%d"%the_object
# Insert the support surface into the planning scene
for obj in find_result.support_surfaces:
# Extend surface to floor
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0],
2.0, # make table wider
obj.primitives[0].dimensions[2] + height]
obj.primitive_poses[0].position.z += -height/2.0
# Add to scene
scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
wait = False)
# Get the table dimensions
table_size = obj.primitives[0].dimensions
# If no objects detected, try again
if the_object == None or target_size is None:
rospy.logerr("Nothing to grasp! try again...")
continue
# Wait for the scene to sync
scene.waitForSync()
# Set colors of the table and the object we are grabbing
scene.setColor(target_id, 223.0/256.0, 90.0/256.0, 12.0/256.0) # orange
scene.setColor(find_result.objects[the_object].object.support_surface, 0.3, 0.3, 0.3, 0.7) # grey
scene.sendColors()
# Skip pick-and-place if we are just detecting objects
if args.objects:
if args.once:
exit(0)
else:
continue
# Get the support surface ID
support_surface = find_result.objects[the_object].object.support_surface
# Set the support surface name to the table object
right_arm.set_support_surface_name(support_surface)
# 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 = target_pose.pose.position.x
place_pose.pose.position.y = 0.03
place_pose.pose.position.z = table_size[2] + target_size[2] / 2.0 + 0.015
place_pose.pose.orientation.w = 1.0
# Initialize the grasp pose to the target pose
grasp_pose = target_pose
# Shift the grasp pose half the size of the target to center it in the gripper
try:
grasp_pose.pose.position.x += target_size[0] / 2.0
grasp_pose.pose.position.y -= 0.01
grasp_pose.pose.position.z += target_size[2] / 2.0
except:
rospy.loginfo("Invalid object size so skipping")
continue
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id])
# Publish the grasp poses so they can be viewed in RViz
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# Track success/failure and number of attempts for pick operation
result = None
n_attempts = 0
# Set the start state to the current state
right_arm.set_start_state_to_current_state()
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
result = right_arm.pick(target_id, grasps)
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
rospy.sleep(1.0)
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
# Generate valid place poses
places = self.make_places(place_pose)
# Set the start state to the current state
#right_arm.set_start_state_to_current_state()
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
for place in places:
result = right_arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
rospy.sleep(0.2)
if result != MoveItErrorCodes.SUCCESS:
rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.")
else:
rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.")
rospy.sleep(2)
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(2)
# Return the arm to the "resting" pose stored in the SRDF file
right_arm.set_named_target('resting')
right_arm.go()
rospy.sleep(2)
# Give the servos a rest
arbotix_relax_all_servos()
rospy.sleep(2)
if args.once:
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
class GripperInterfaceClient:
def __init__(self):
self.joint_names = ["l_gripper_finger_joint", "r_gripper_finger_joint"]
self.client = actionlib.SimpleActionClient("gripper_controller/gripper_action", GripperCommandAction)
rospy.loginfo("Waiting for gripper_controller...")
self.client.wait_for_server()
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True, plan_only=True)
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
def execute_trajectory(self, trajectory):
goal_position = trajectory.points[-1].positions
print "==========", goal_position
command_goal = GripperCommandGoal()
command_goal.command.position = goal_position[0] + goal_position[1]
command_goal.command.max_effort = 50.0 # Placeholder. TODO Figure out a better way to compute this
self.client.send_goal(command_goal)
self.client.wait_for_result()
def move_to(self, goal_position, max_effort):
command_goal = GripperCommandGoal()
command_goal.command.position = goal_position
command_goal.command.max_effort = max_effort
self.client.send_goal(command_goal)
self.client.wait_for_result()
def updateScene(self):
# find objects
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
find_result = self.find_client.get_result()
# 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()
# insert objects to scene
idx = -1
for obj in find_result.objects:
idx += 1
obj.object.name = "object%d" % idx
self.scene.addSolidPrimitive(
obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], wait=False
)
# Try mesh representation
# idx = -1
# for obj in find_result.objects:
# idx += 1
# obj.object.name = "object%d"%idx
# self.scene.addMesh(obj.object.name,
# obj.object.mesh_poses[0],
# obj.object.meshes[0],
# wait = False)
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [
obj.primitives[0].dimensions[0] + 0.1,
2.5, # wider
obj.primitives[0].dimensions[2] + height,
]
obj.primitive_poses[0].position.z += -height / 2.0
# add to scene
self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], wait=False)
self.scene.waitForSync()
# store for grasping
self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
def getGraspableCube(self):
graspable = None
for obj in self.objects:
# need grasps
if len(obj.grasps) < 1:
continue
# check size
# if obj.object.primitives[0].dimensions[0] < 0.03 or \
# obj.object.primitives[0].dimensions[0] > 0.04 or \
# obj.object.primitives[0].dimensions[0] < 0.07 or \
# obj.object.primitives[0].dimensions[0] > 0.09 or \
# obj.object.primitives[2].dimensions[2] < 0.19 or \
# obj.object.primitives[2].dimensions[2] > 0.20:
# continue
# has to be on table
# if obj.object.primitive_poses[0].position.z < 0.5:
# continue
return obj.object, obj.grasps
# nothing detected
return None, None
def plan_pick(self, block, grasps):
success, pick_result = self.pickplace.pick_with_retry(
block.name,
grasps,
support_name=block.support_surface,
allow_gripper_support_collision=False,
planner_id="PRMkConfigDefault",
retries=2,
scene=self.scene,
)
# self.pick_result = pick_result
if success:
return pick_result.trajectory_stages
else:
rospy.loginfo("Planning Failed.")
return None
class GraspingClient(object):
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.move_group = MoveGroupInterface("arm", "base_link")
self.objectname_sub = rospy.Subscriber("/objects", Float32MultiArray,
self.callback)
self.interestedObjectName = ""
#self.objects = []
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(
find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
def callback(self, msg):
if len(msg.data) != 0:
self.interestedObjectName = "/object_" + (str(int(msg.data[0])))
#print(self.interestedObjectName)
def getInterestedObject(self, objects):
listener = tf.TransformListener()
while not rospy.is_shutdown():
try:
if len(self.interestedObjectName) == 0:
continue
(trans,
rot) = listener.lookupTransform("/base_link",
self.interestedObjectName,
rospy.Time(0))
if len(trans) == 3:
break
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
final_objects = []
for obj in objects:
print(obj[0].object.name)
print("obj_x: " + str(trans[0]) + "----" +
str(obj[0].object.primitive_poses[0].position.x))
print("obj_x: " + str(trans[1]) + "----" +
str(obj[0].object.primitive_poses[0].position.y))
print("obj_x: " + str(trans[2]) + "----" +
str(obj[0].object.primitive_poses[0].position.z))
if (abs(obj[0].object.primitive_poses[0].position.x - trans[0]) <=
0.1 and abs(obj[0].object.primitive_poses[0].position.y -
trans[1]) <= 0.1
and abs(obj[0].object.primitive_poses[0].position.z -
trans[2]) <= 0.1):
return [obj[0]]
break
return []
rospy.loginfo("No Interested Objects found")
def updateScene(self):
# find objects
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
find_result = self.find_client.get_result()
# 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()
# insert objects to scene
objects = list()
idx = -1
for obj in find_result.objects:
idx += 1
obj.object.name = "object%d" % idx
temp_height = obj.object.primitive_poses[0].position.z
obj.object.primitive_poses[0].position.z -= 0.02 * temp_height
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
wait=False)
if obj.object.primitive_poses[0].position.x < 0.85:
objects.append([obj, obj.object.primitive_poses[0].position.z])
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [
obj.primitives[0].dimensions[0],
1.5, # wider
obj.primitives[0].dimensions[2] + height
]
obj.primitive_poses[0].position.z += -height / 2.0
# add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
wait=False)
self.scene.waitForSync()
# store for grasping
#self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
# store graspable objects by Z
#objects.sort(key=lambda object: object[1])
#objects.reverse()
#self.objects = [object[0] for object in objects]
self.objects = self.getInterestedObject(objects)
#for object in objects:
# print(object[0].object.name, object[1])
#exit(-1)
def getGraspableObject(self):
graspable = None
if len(self.objects) == 0:
return None, None
for obj in self.objects:
# need grasps
if len(obj.grasps) < 1:
continue
# check size
if obj.object.primitives[0].dimensions[0] < 0.03 or \
obj.object.primitives[0].dimensions[0] < 0.03 or \
obj.object.primitives[0].dimensions[0] < 0.03:
continue
# has to be on table
if obj.object.primitive_poses[0].position.z < 0.5:
continue
print obj.object.primitive_poses[0], obj.object.primitives[0]
return obj.object, obj.grasps
# nothing detected
return None, None
def getSupportSurface(self, name):
for surface in self.support_surfaces:
if surface.name == name:
return surface
return None
def getPlaceLocation(self):
pass
def pick(self, block, grasps):
success, pick_result = self.pickplace.pick_with_retry(
block.name,
grasps,
support_name=block.support_surface,
scene=self.scene)
self.pick_result = pick_result
return success
def place(self, block, pose_stamped):
places = list()
l = PlaceLocation()
l.place_pose.pose = pose_stamped.pose
l.place_pose.header.frame_id = pose_stamped.header.frame_id
# copy the posture, approach and retreat from the grasp used
l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach
l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
places.append(copy.deepcopy(l))
# create another several places, rotate each by 360/m degrees in yaw direction
m = 16 # number of possible place poses
pi = 3.141592653589
for i in range(0, m - 1):
l.place_pose.pose = rotate_pose_msg_by_euler_angles(
l.place_pose.pose, 0, 0, 2 * pi / m)
places.append(copy.deepcopy(l))
success, place_result = self.pickplace.place_with_retry(
block.name,
places,
support_name=block.support_surface,
scene=self.scene)
return success
def tuck(self):
joints = [
"shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def stow(self):
joints = [
"shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
pose = [1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def intermediate_stow(self):
joints = [
"shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
pose = [0.7, -0.3, 0.0, -0.3, 0.0, -0.57, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
class World:
def __init__(self, tower_array, disk_array, config, debug=0):
print "[INFO] Initialise World"
self.DEBUG = debug
# initialise arrays
self.tower_array = tower_array
self.disk_array = disk_array
# configs
self.max_gripper = config.get_max_gripper()
self.disk_height = config.disk_height
self.tower_positions = config.tower_positions
self.planning_scene_interface = PlanningSceneInterface(REFERENCE_FRAME)
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
"""Calls a method to display the collision objects.
"""
def create_planning_scene(self):
print "[INFO] Create_planning_scene"
self.display_towers_and_disks()
"""This method collects all needed information and
publish them to other methods.
"""
def display_towers_and_disks(self):
print "[INFO] Display towers and disks"
for tower in self.tower_array:
# call method to publish new tower
self.publish_scene(tower.get_position(), tower)
# set color by name
self.planning_scene_interface.setColor(tower.get_name(), 1.0, 1.0, 1.0)
# publish color
self.planning_scene_interface.sendColors()
for disk in self.disk_array:
self.publish_scene(disk.get_position(), None, disk)
self.planning_scene_interface.setColor(disk.get_id(), disk.get_color()[0], disk.get_color()[1],
disk.get_color()[2], disk.get_color()[3])
self.planning_scene_interface.sendColors()
# wait for sync after publishing collision objects
self.planning_scene_interface.waitForSync(5.0)
rospy.sleep(5)
"""This method creates a box or a cylinder with methods of
the planning scene interface.
:param position: the position of the new collision object
:param tower: the tower object
:param disk: the disk object
"""
def publish_scene(self, position, tower=None, disk=None):
if tower is not None:
self.planning_scene_interface.addBox(tower.get_name(), self.max_gripper / 100.0, self.max_gripper / 100.0,
(self.tower_positions[tower.get_id() - 1][2] * 2), position[0],
position[1], position[2])
else:
self.planning_scene_interface.addCylinder(disk.get_id(), self.disk_height, disk.get_diameter() / 2,
position[0], position[1], position[2])
"""This method cleans the planning scene.
:param close: with this flag a new planning scene objects will be removed
in sync otherwise the object will be deleted without syncing the scene
"""
def clean_up(self, close=False):
if close:
# get the actual list after game
self.planning_scene_interface = PlanningSceneInterface(REFERENCE_FRAME)
for name in self.planning_scene_interface.getKnownCollisionObjects():
if self.DEBUG is 1:
print("[DEBUG] Removing object %s" % name)
self.planning_scene_interface.removeCollisionObject(name, False)
for name in self.planning_scene_interface.getKnownAttachedObjects():
if self.DEBUG is 1:
print("[DEBUG] Removing attached object %s" % name)
self.planning_scene_interface.removeAttachedObject(name, False)
if close:
self.planning_scene_interface.waitForSync(5.0)
pass
"""This method corrects the position of the moved disk.
:param tower: parent tower of the disk
"""
def refresh_disk_pose(self, tower):
print "[INFO] Refresh disk pose"
disk = tower.get_last()
if self.DEBUG is 1:
print "[DEBUG] Refresh", disk.get_id(), "pose:", disk.get_position(), "tower size", tower.get_size(),\
"tower pose", tower.get_position()
# remove the disk from planning scene
self.planning_scene_interface.removeCollisionObject(disk.get_id(), False)
# publish collision object and set old color
self.publish_scene(disk.get_position(), None, disk)
self.planning_scene_interface.setColor(disk.get_id(), disk.get_color()[0], disk.get_color()[1],
disk.get_color()[2], disk.get_color()[3])
self.planning_scene_interface.sendColors()
class MoveItDemo:
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# We need a tf2 listener to convert poses into arm reference base
try:
self._tf2_buff = tf2_ros.Buffer()
self._tf2_list = tf2_ros.TransformListener(self._tf2_buff)
except rospy.ROSException as err:
rospy.logerr("MoveItDemo: could not start tf buffer client: " + str(err))
raise err
self.gripper_opened = [rospy.get_param(GRIPPER_PARAM + "/max_opening") - 0.01]
self.gripper_closed = [rospy.get_param(GRIPPER_PARAM + "/min_opening") + 0.01]
self.gripper_neutral = [rospy.get_param(GRIPPER_PARAM + "/neutral",
(self.gripper_opened[0] + self.gripper_closed[0])/2.0) ]
self.gripper_tighten = rospy.get_param(GRIPPER_PARAM + "/tighten", 0.0)
# Use the planning scene object to add or remove objects
self.scene = PlanningSceneInterface(REFERENCE_FRAME)
# 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('target_pose', PoseStamped, queue_size=5)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the right arm
arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
arm.set_goal_position_tolerance(0.05)
arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame
arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
arm.set_planning_time(15)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 1
# Set a limit on the number of place attempts
max_place_attempts = 3
# Give the scene a chance to catch up
rospy.sleep(2)
# Connect to the simple_grasping find_objects action server
rospy.loginfo("Connecting to basic_grasping_perception/find_objects...")
find_objects = actionlib.SimpleActionClient("basic_grasping_perception/find_objects", FindGraspableObjectsAction)
find_objects.wait_for_server()
rospy.loginfo("...connected")
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
#right_arm.set_named_target('resting')
#right_arm.go()
# Open the gripper to the neutral position
arm.set_named_target('right_up')
if arm.go() != True:
rospy.logwarn(" Go failed")
gripper.set_joint_value_target(self.gripper_opened)
if gripper.go() != True:
rospy.logwarn(" Go failed")
rospy.sleep(1)
while not rospy.is_shutdown():
# Initialize the grasping goal
goal = FindGraspableObjectsGoal()
# We don't use the simple_grasping grasp planner as it does not work with our gripper
goal.plan_grasps = False
# Send the goal request to the find_objects action server which will trigger
# the perception pipeline
find_objects.send_goal(goal)
# Wait for a result
find_objects.wait_for_result(rospy.Duration(5.0))
# The result will contain support surface(s) and objects(s) if any are detected
find_result = find_objects.get_result()
# Display the number of objects found
rospy.loginfo("Found %d objects" % len(find_result.objects))
# Remove all previous objects from the planning scene
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()
# Clear the virtual object colors
self.scene._colors = dict()
# Use the nearest object on the table as the target
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_id = 'target'
target_size = None
the_object = None
the_object_dist = 0.30
the_object_dist_min = 0.10
count = -1
for obj in find_result.objects:
count += 1
self.scene.addSolidPrimitive("object%d"%count,
obj.object.primitives[0],
obj.object.primitive_poses[0],
wait = False)
# Choose the object nearest to the robot
dx = obj.object.primitive_poses[0].position.x
dy = obj.object.primitive_poses[0].position.y
d = math.sqrt((dx * dx) + (dy * dy))
if d < the_object_dist and d > the_object_dist_min:
rospy.loginfo("object is in the working zone")
the_object_dist = d
the_object = count
target_size = [0.02, 0.02, 0.05]
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = obj.object.primitive_poses[0].position.x + target_size[0] / 2.0
target_pose.pose.position.y = obj.object.primitive_poses[0].position.y
target_pose.pose.position.z = 0.04
target_pose.pose.orientation.x = 0.0
target_pose.pose.orientation.y = 0.0
target_pose.pose.orientation.z = 0.0
target_pose.pose.orientation.w = 1.0
else:
rospy.loginfo("object is not in the working zone")
rospy.sleep(1)
# Make sure we found at least one object before setting the target ID
if the_object != None:
target_id = "object%d"%the_object
# Insert the support surface into the planning scene
for obj in find_result.support_surfaces:
# Extend surface to floor
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0],
2.0, # make table wider
obj.primitives[0].dimensions[2] - height]
obj.primitive_poses[0].position.z += -height/2.0
# Add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
wait = False)
# Get the table dimensions
table_size = obj.primitives[0].dimensions
# If no objects detected, try again
if the_object == None or target_size is None:
rospy.logerr("Nothing to grasp! try again...")
continue
# Wait for the scene to sync
self.scene.waitForSync()
# Set colors of the table and the object we are grabbing
self.scene.setColor(target_id, 223.0/256.0, 90.0/256.0, 12.0/256.0) # orange
self.scene.setColor(find_result.objects[the_object].object.support_surface, 0.3, 0.3, 0.3, 0.7) # grey
self.scene.sendColors()
if args.objects:
if args.once:
exit(0)
else:
continue
# Get the support surface ID
support_surface = find_result.objects[the_object].object.support_surface
# Set the support surface name to the table object
arm.set_support_surface_name(support_surface)
# Initialize the grasp pose to the target pose
grasp_pose = target_pose
# Shift the grasp pose half the size of the target to center it in the gripper
try:
grasp_pose.pose.position.x += target_size[0] / 2.0
grasp_pose.pose.position.y -= 0.01
grasp_pose.pose.position.z += target_size[2] / 2.0
except:
rospy.loginfo("Invalid object size so skipping")
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id], [target_size[1] - self.gripper_tighten])
# Publish the grasp poses so they can be viewed in RViz
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# Track success/failure and number of attempts for pick operation
result = None
n_attempts = 0
# Set the start state to the current state
arm.set_start_state_to_current_state()
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
result = arm.pick(target_id, grasps)
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
rospy.sleep(1.0)
rospy.sleep(2)
# Open the gripper to the neutral position
gripper.set_joint_value_target(self.gripper_neutral)
gripper.go()
rospy.sleep(2)
# Return the arm to the "resting" pose stored in the SRDF file
arm.set_named_target('right_up')
arm.go()
rospy.sleep(2)
if args.once:
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
# Get the gripper posture as a JointTrajectory
def make_gripper_posture(self, joint_positions):
# Initialize the joint trajectory for the gripper joints
t = JointTrajectory()
# Set the joint names to the gripper joint names
t.header.stamp = rospy.get_rostime()
t.joint_names = GRIPPER_JOINT_NAMES
# Initialize a joint trajectory point to represent the goal
tp = JointTrajectoryPoint()
# Assign the trajectory joint positions to the input positions
tp.positions = joint_positions
# Set the gripper effort
tp.effort = GRIPPER_EFFORT
tp.time_from_start = rospy.Duration(1.0)
# Append the goal point to the trajectory points
t.points.append(tp)
# Return the joint trajectory
return t
# Generate a gripper translation in the direction given by vector
def make_gripper_translation(self, min_dist, desired, vector):
# Initialize the gripper translation object
g = GripperTranslation()
# Set the direction vector components to the input
g.direction.vector.x = vector[0]
g.direction.vector.y = vector[1]
g.direction.vector.z = vector[2]
# The vector is relative to the gripper frame
g.direction.header.frame_id = GRIPPER_FRAME
# Assign the min and desired distances from the input
g.min_distance = min_dist
g.desired_distance = desired
return g
# Generate a list of possible grasps
def make_grasps(self, initial_pose_stamped, allowed_touch_objects, grasp_opening=[0]):
# Initialize the grasp object
g = Grasp()
# Set the pre-grasp and grasp postures appropriately;
# grasp_opening should be a bit smaller than target width
g.pre_grasp_posture = self.make_gripper_posture(self.gripper_opened)
g.grasp_posture = self.make_gripper_posture(grasp_opening)
# Set the approach and retreat parameters as desired
g.pre_grasp_approach = self.make_gripper_translation(0.1, 0.15, [0.0, 0.0, -1.5])
g.post_grasp_retreat = self.make_gripper_translation(0.1, 0.15, [0.0, 0.0, 1.5])
# Set the first grasp pose to the input pose
g.grasp_pose = initial_pose_stamped
# Pitch angles to try
pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.4, -0.4, 0.5, -0.5, 0.6, -0.6, 0.7, -0.7, 0.8, -0.8, 0.9, -0.9, 1.0, -1.0, 1.1, -1.1, 1.2, -1.2, 1.3, -1.3, 1.4, -1.4, 1.5, -1.5]
# Yaw angles to try; given the limited dofs of turtlebot_arm, we must calculate the heading
# from arm base to the object to pick (first we must transform its pose to arm base frame)
target_pose_arm_ref = self._tf2_buff.transform(initial_pose_stamped, ARM_BASE_FRAME)
x = target_pose_arm_ref.pose.position.x
y = target_pose_arm_ref.pose.position.y
yaw_vals = [atan2(y, x) + inc for inc in [0, 0.1,-0.1]]
# A list to hold the grasps
grasps = []
# Generate a grasp for each pitch and yaw angle
for yaw in yaw_vals:
for pitch in pitch_vals:
# Create a quaternion from the Euler angles
q = quaternion_from_euler(0, pitch, yaw)
# Set the grasp pose orientation accordingly
g.grasp_pose.pose.orientation.x = q[0]
g.grasp_pose.pose.orientation.y = q[1]
g.grasp_pose.pose.orientation.z = q[2]
g.grasp_pose.pose.orientation.w = q[3]
# Set and id for this grasp (simply needs to be unique)
g.id = str(len(grasps))
# Set the allowed touch objects to the input list
g.allowed_touch_objects = allowed_touch_objects
# Don't restrict contact force
g.max_contact_force = 0
# Degrade grasp quality for increasing pitch angles
g.grasp_quality = 1.0 - abs(pitch)
# Append the grasp to the list
grasps.append(deepcopy(g))
# Return the list
return grasps
# 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.005] #, 0.01, -0.01, 0.015, -0.015]
# A list of y shifts (meters) to try
y_vals = [0, 0.005, -0.005, 0.01, -0.01] #, 0.015, -0.015]
# A list of pitch angles to try
pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.4, -0.4, 0.5, -0.5, 0.6, -0.6, 0.7, -0.7, 0.8, -0.8, 0.9, -0.9, 1.0, -1.0, 1.1, -1.1, 1.2, -1.2, 1.3, -1.3, 1.4, -1.4, 1.5, -1.5] #, 0.005, -0.005, 0.01, -0.01, 0.02, -0.02]
# A list to hold the places
places = []
# Generate a place pose for each angle and translation
for pitch in pitch_vals:
for dy in y_vals:
for dx in x_vals:
place.pose.position.x = init_pose.pose.position.x + dx
place.pose.position.y = init_pose.pose.position.y + dy
# Yaw angle: given the limited dofs of turtlebot_arm, we must calculate the heading from
# arm base to the place location (first we must transform its pose to arm base frame)
target_pose_arm_ref = self._tf2_buff.transform(place, ARM_BASE_FRAME)
x = target_pose_arm_ref.pose.position.x
y = target_pose_arm_ref.pose.position.y
yaw = atan2(y, x)
# Create a quaternion from the Euler angles
q = quaternion_from_euler(0, pitch, yaw)
# 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]
# MoveGroup::place will transform the provided place pose with the attached body pose, so the object retains
# the orientation it had when picked. However, with our 4-dofs arm this is infeasible (nor we care about the
# objects orientation!), so we cancel this transformation. It is applied here:
# https://github.com/ros-planning/moveit_ros/blob/jade-devel/manipulation/pick_place/src/place.cpp#L64
# More details on this issue: https://github.com/ros-planning/moveit_ros/issues/577
acobjs = self.scene.get_attached_objects([target_id])
aco_pose = self.get_pose(acobjs[target_id])
if aco_pose is None:
rospy.logerr("Attached collision object '%s' not found" % target_id)
return None
aco_tf = self.pose_to_mat(aco_pose)
place_tf = self.pose_to_mat(place.pose)
place.pose = self.mat_to_pose(place_tf, aco_tf)
rospy.logdebug("Compensate place pose with the attached object pose [%s]. Results: [%s]" \
% (aco_pose, place.pose))
# Append this place pose to the list
places.append(deepcopy(place))
# Return the list
return places
# 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 get_pose(self, co):
# We get object's pose from the mesh/primitive poses; try first with the meshes
if isinstance(co, CollisionObject):
if co.mesh_poses:
return co.mesh_poses[0]
elif co.primitive_poses:
return co.primitive_poses[0]
else:
rospy.logerr("Collision object '%s' has no mesh/primitive poses" % co.id)
return None
elif isinstance(co, AttachedCollisionObject):
if co.object.mesh_poses:
return co.object.mesh_poses[0]
elif co.object.primitive_poses:
return co.object.primitive_poses[0]
else:
rospy.logerr("Attached collision object '%s' has no mesh/primitive poses" % co.id)
return None
else:
rospy.logerr("Input parameter is not a collision object")
return None
def pose_to_mat(self, pose):
'''Convert a pose message to a 4x4 numpy matrix.
Args:
pose (geometry_msgs.msg.Pose): Pose rospy message class.
Returns:
mat (numpy.matrix): 4x4 numpy matrix
'''
quat = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]
pos = np.matrix([pose.position.x, pose.position.y, pose.position.z]).T
mat = np.matrix(quaternion_matrix(quat))
mat[0:3, 3] = pos
return mat
def mat_to_pose(self, mat, transform=None):
'''Convert a homogeneous matrix to a Pose message, optionally premultiply by a transform.
Args:
mat (numpy.ndarray): 4x4 array (or matrix) representing a homogenous transform.
transform (numpy.ndarray): Optional 4x4 array representing additional transform
Returns:
pose (geometry_msgs.msg.Pose): Pose message representing transform.
'''
if transform != None:
mat = np.dot(mat, transform)
pose = Pose()
pose.position.x = mat[0,3]
pose.position.y = mat[1,3]
pose.position.z = mat[2,3]
quat = quaternion_from_matrix(mat)
pose.orientation.x = quat[0]
pose.orientation.y = quat[1]
pose.orientation.z = quat[2]
pose.orientation.w = quat[3]
return pose
class GraspingClient(object):
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.move_group = MoveGroupInterface("arm", "base_link")
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
def updateScene(self):
# find objects
# here i am declaring goal as an object. https://github.com/mikeferguson/grasping_msgs/blob/master/action/FindGraspableObjects.action
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
# passing the object to find_client
# now find_client is wer magic happens
# on demo.launch file i am runnning basic_grasping_perception. (below <!-- Start Perception -->)
# this keeps running on background and i use actionlib (initalize on init) to get a hook to it.
# find_client is connected to basic_grasping_perception
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
# here we get all the objects
find_result = self.find_client.get_result()
# 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()
rospy.loginfo("updating scene")
idx = 0
# insert objects to the planning scene
#TODO so these two for loops yo can hardcode the values. try printing all the params and u will understand
for obj in find_result.objects:
rospy.loginfo("object number -> %d" %idx)
obj.object.name = "object%d"%idx
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
wait = False)
idx += 1
# for grp in obj.grasps:
# grp.grasp_pose.pose.position.z = 0.37
# this is just minor adjustments i did mess up with this code. just follwed simple gasp thingy
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
height = obj.primitive_poses[0].position.z
rospy.loginfo("height before => %f" % height)
obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0],
1.5, # wider
obj.primitives[0].dimensions[2] + height]
obj.primitive_poses[0].position.z += -height/2.0
rospy.loginfo("height after => %f" % obj.primitive_poses[0].position.z)
# add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
wait = False)
self.scene.waitForSync()
# store for grasping
self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
def getGraspableCube(self):
graspable = None
for obj in self.objects:
# need grasps
rospy.loginfo("no. of grasps detected %d dim => %f" % (len(obj.grasps), obj.object.primitives[0].dimensions[0]))
if len(obj.grasps) < 1:
continue
# check size our object is a 0.05^3 cube
if obj.object.primitives[0].dimensions[0] < 0.04 or \
obj.object.primitives[0].dimensions[0] > 0.06 or \
obj.object.primitives[0].dimensions[1] < 0.04 or \
obj.object.primitives[0].dimensions[1] > 0.06 or \
obj.object.primitives[0].dimensions[2] < 0.04 or \
obj.object.primitives[0].dimensions[2] > 0.06:
continue
rospy.loginfo("###### size: x => %f, y => %f, z => %f" % (obj.object.primitive_poses[0].position.x,obj.object.primitive_poses[0].position.y,obj.object.primitive_poses[0].position.z))
# has to be on table
#if obj.object.primitive_poses[0].position.z < 0.5:grasping_client
# continue
return obj.object, obj.grasps
# nothing detected
rospy.loginfo("nothing detected")
return None, None
def getSupportSurface(self, name):
for surface in self.support_surfaces:
if surface.name == name:
return surface
return None
def getPlaceLocation(self):
pass
def pick(self, block, grasps):
success, pick_result = self.pickplace.pick_with_retry(block.name,
grasps,
support_name=block.support_surface,
scene=self.scene)
self.pick_result = pick_result
return success
def place(self, block, pose_stamped):
places = list()
l = PlaceLocation()
l.place_pose.pose = pose_stamped.pose
l.place_pose.header.frame_id = pose_stamped.header.frame_id
# copy the posture, approach and retreat from the grasp used
l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach
l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
places.append(copy.deepcopy(l))
# create another several places, rotate each by 360/m degrees in yaw direction
m = 16 # number of possible place poses
pi = 3.141592653589
for i in range(0, m-1):
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 2 * pi / m)
places.append(copy.deepcopy(l))
success, place_result = self.pickplace.place_with_retry(block.name,
places,
scene=self.scene)
return success
def tuck(self):
# so for each joint i will pass a specific value. first arms are move to pose1 then pose2 then pose3 sequentially
joints = ["shoulder_roll_joint", "shoulder_pitch_joint", "shoulder_yaw_joint",
"elbow_pitch_joint", "elbow_yaw_joint", "wrist_pitch_joint", "wrist_roll_joint"]
pose1 = [1.57079633, 0, 0, 0, 0, 0, 0.0]
pose2 = [1.57079633, 0, -1.57079633, 0, -1.57079633, 0, 0.0]
pose3 = [1.57079633, 1.57079633, -1.57079633, 0, -1.57079633, 1.57079633, 0.0]
while not rospy.is_shutdown():
self.move_group.moveToJointPosition(joints, pose1, 0.02)
self.move_group.moveToJointPosition(joints, pose2, 0.02)
result = self.move_group.moveToJointPosition(joints, pose3, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
class GraspingClient(object):
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.move_group = MoveGroupInterface("arm", "base_link")
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
def updateScene(self):
# find objects
rospy.loginfo("get new grasps")
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
find_result = self.find_client.get_result()
# 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.removeCollisionObject("my_front_ground")
self.scene.removeCollisionObject("my_back_ground")
self.scene.removeCollisionObject("my_right_ground")
self.scene.removeCollisionObject("my_left_ground")
self.scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
self.scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
self.scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
self.scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
# TODO: ADD BOX
self.scene.waitForSync()
# insert objects to scene
idx = -1
for obj in find_result.objects:
idx += 1
obj.object.name = "object%d"%idx
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
wait = False)
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
height = obj.primitive_poses[0].position.z
# added + .1
obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0],
1.5, # wider
obj.primitives[0].dimensions[2] + height]
obj.primitive_poses[0].position.z += -height/2.0
# add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
wait = False)
self.scene.waitForSync()
# store for grasping
self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
def getSupportSurface(self, name):
for surface in self.support_surfaces:
if surface.name == name:
return surface
return None
def tuck(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def zero(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
class World:
def __init__(self, tower_array, disk_array, config, debug=0):
print "[INFO] Initialise World"
self.DEBUG = debug
# initialise arrays
self.tower_array = tower_array
self.disk_array = disk_array
# configs
self.max_gripper = config.get_max_gripper()
self.disk_height = config.disk_height
self.tower_positions = config.tower_positions
self.planning_scene_interface = PlanningSceneInterface(REFERENCE_FRAME)
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=10)
"""Calls a method to display the collision objects.
"""
def create_planning_scene(self):
print "[INFO] Create_planning_scene"
self.display_towers_and_disks()
"""This method collects all needed information and
publish them to other methods.
"""
def display_towers_and_disks(self):
print "[INFO] Display towers and disks"
for tower in self.tower_array:
# call method to publish new tower
self.publish_scene(tower.get_position(), tower)
# set color by name
self.planning_scene_interface.setColor(tower.get_name(), 1.0, 1.0,
1.0)
# publish color
self.planning_scene_interface.sendColors()
for disk in self.disk_array:
self.publish_scene(disk.get_position(), None, disk)
self.planning_scene_interface.setColor(disk.get_id(),
disk.get_color()[0],
disk.get_color()[1],
disk.get_color()[2],
disk.get_color()[3])
self.planning_scene_interface.sendColors()
# wait for sync after publishing collision objects
self.planning_scene_interface.waitForSync(5.0)
rospy.sleep(5)
"""This method creates a box or a cylinder with methods of
the planning scene interface.
:param position: the position of the new collision object
:param tower: the tower object
:param disk: the disk object
"""
def publish_scene(self, position, tower=None, disk=None):
if tower is not None:
self.planning_scene_interface.addBox(
tower.get_name(), self.max_gripper / 100.0,
self.max_gripper / 100.0,
(self.tower_positions[tower.get_id() - 1][2] * 2), position[0],
position[1], position[2])
else:
self.planning_scene_interface.addCylinder(disk.get_id(),
self.disk_height,
disk.get_diameter() / 2,
position[0], position[1],
position[2])
"""This method cleans the planning scene.
:param close: with this flag a new planning scene objects will be removed
in sync otherwise the object will be deleted without syncing the scene
"""
def clean_up(self, close=False):
if close:
# get the actual list after game
self.planning_scene_interface = PlanningSceneInterface(
REFERENCE_FRAME)
for name in self.planning_scene_interface.getKnownCollisionObjects():
if self.DEBUG is 1:
print("[DEBUG] Removing object %s" % name)
self.planning_scene_interface.removeCollisionObject(name, False)
for name in self.planning_scene_interface.getKnownAttachedObjects():
if self.DEBUG is 1:
print("[DEBUG] Removing attached object %s" % name)
self.planning_scene_interface.removeAttachedObject(name, False)
if close:
self.planning_scene_interface.waitForSync(5.0)
pass
"""This method corrects the position of the moved disk.
:param tower: parent tower of the disk
"""
def refresh_disk_pose(self, tower):
print "[INFO] Refresh disk pose"
disk = tower.get_last()
if self.DEBUG is 1:
print "[DEBUG] Refresh", disk.get_id(), "pose:", disk.get_position(), "tower size", tower.get_size(),\
"tower pose", tower.get_position()
# remove the disk from planning scene
self.planning_scene_interface.removeCollisionObject(
disk.get_id(), False)
# publish collision object and set old color
self.publish_scene(disk.get_position(), None, disk)
self.planning_scene_interface.setColor(disk.get_id(),
disk.get_color()[0],
disk.get_color()[1],
disk.get_color()[2],
disk.get_color()[3])
self.planning_scene_interface.sendColors()
class GraspingClient(object):
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.move_group = MoveGroupInterface("arm", "base_link")
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
obj_cts = 0
def updateScene(self):
# find objects
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
print " find goal"
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
print " find client"
find_result = self.find_client.get_result()
# remove previous objects
for name in self.scene.getKnownCollisionObjects():
print "the object %s should be removed" %(name)
self.scene.removeCollisionObject(name, False)
for name in self.scene.getKnownAttachedObjects():
print "the object %s should be removed" %(name)
self.scene.removeAttachedObject(name, False)
self.scene.waitForSync()
# insert objects to scene
objects = list()
idx = -1
for obj in find_result.objects:
idx += 1
obj.object.name = "object%d"%idx
print "-----------------------"
print obj.object.primitive_poses[0]
print " x: %d" %(obj.object.primitive_poses[0].position.x)
# if obj.object.primitive_poses[0].position.y > 0.0
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
use_service = False)
# def addSolidPrimitive (self, name, solid, pose, use_service=True)
print "1, %d" %(idx)
if obj.object.primitive_poses[0].position.x < 1.25:
objects.append([obj, obj.object.primitive_poses[0].position.z])
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
print "2,"
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0],
1.5, # wider
obj.primitives[0].dimensions[2] + height]
obj.primitive_poses[0].position.z += -height/2.0
# add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
use_service = True
)
self.scene.waitForSync()
# store for grasping
#self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
# store graspable objects by Z
objects.sort(key=lambda object: object[1])
objects.reverse()
self.objects = [object[0] for object in objects]
rospy.loginfo("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
rospy.loginfo("number of objects...:::::::" + str(len(objects)))
#for object in objects:
# print(object[0].object.name, object[1])
#exit(-1)
def getGraspableObject(self):
graspable = None
for obj in self.objects:
# need grasps
if len(obj.grasps) < 1:
rospy.loginfo("must more than one object")
continue
# check size
if obj.object.primitives[0].dimensions[0] < 0.03 or \
obj.object.primitives[0].dimensions[0] > 0.25 or \
obj.object.primitives[0].dimensions[0] < 0.03 or \
obj.object.primitives[0].dimensions[0] > 0.25 or \
obj.object.primitives[0].dimensions[0] < 0.03 or \
obj.object.primitives[0].dimensions[0] > 0.25:
continue
# has to located in +y
if obj.object.primitive_poses[0].position.y < 0.0:
print "has to located in +y"
continue
# has to be on table
if obj.object.primitive_poses[0].position.z < 0.5:
print "z has to larger than 0.5 "
continue
rospy.loginfo("object pose:")
print obj.object.primitive_poses[0], obj.object.primitives[0]
return obj.object, obj.grasps
# nothing detected
return None, None
def getSupportSurface(self, name):
for surface in self.support_surfaces:
if surface.name == name:
return surface
return None
def getPlaceLocation(self):
pass
def pick(self, block, grasps):
success, pick_result = self.pickplace.pick_with_retry(block.name,
grasps,
support_name=block.support_surface,
scene=self.scene)
self.pick_result = pick_result
return success
def place(self, block, pose_stamped):
places = list()
l = PlaceLocation()
l.place_pose.pose = pose_stamped.pose
l.place_pose.header.frame_id = pose_stamped.header.frame_id
# copy the posture, approach and retreat from the grasp used
l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach
l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
places.append(copy.deepcopy(l))
# create another several places, rotate each by 360/m degrees in yaw direction
m = 16 # number of possible place poses
pi = 3.141592653589
for i in range(0, m-1):
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 2 * pi / m)
places.append(copy.deepcopy(l))
success, place_result = self.pickplace.place_with_retry(block.name,
places,
scene=self.scene)
return success
def tuck(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def stow(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def intermediate_stow(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [0.7, -0.3, 0.0, -0.3, 0.0, -0.57, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
class GraspingClient(object):
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.move_group = MoveGroupInterface("arm", "base_link")
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(
find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
def updateScene(self):
# find objects
rospy.loginfo("get new grasps")
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
find_result = self.find_client.get_result()
# 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.removeCollisionObject("my_front_ground")
self.scene.removeCollisionObject("my_back_ground")
self.scene.removeCollisionObject("my_right_ground")
self.scene.removeCollisionObject("my_left_ground")
self.scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
self.scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
self.scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
self.scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
# TODO: ADD BOX
self.scene.waitForSync()
# insert objects to scene
idx = -1
for obj in find_result.objects:
idx += 1
obj.object.name = "object%d" % idx
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
wait=False)
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
height = obj.primitive_poses[0].position.z
# added + .1
obj.primitives[0].dimensions = [
obj.primitives[0].dimensions[0],
1.5, # wider
obj.primitives[0].dimensions[2] + height
]
obj.primitive_poses[0].position.z += -height / 2.0
# add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
wait=False)
self.scene.waitForSync()
# store for grasping
self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
def getSupportSurface(self, name):
for surface in self.support_surfaces:
if surface.name == name:
return surface
return None
def tuck(self):
joints = [
"shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def zero(self):
joints = [
"shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def picknplace():
# Initialize the planning scene interface.
p = PlanningSceneInterface("base")
# Connect the arms to the move group.
g = MoveGroupInterface("both_arms", "base")
gr = MoveGroupInterface("right_arm", "base")
gl = MoveGroupInterface("left_arm", "base")
# Create baxter_interface limb instance.
leftarm = baxter_interface.limb.Limb('left')
# Create baxter_interface gripper instance.
leftgripper = baxter_interface.Gripper('left')
leftgripper.calibrate()
leftgripper.open()
# Define the joints for the positions.
jts_both = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2', 'right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
jts_right = ['right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
jts_left = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2']
pos1 = [-1.4580487388850858, 1.627553615946424, 0.07669903939427068, -0.3539660668045592, -1.9155585088719105, -1.4124128104454947, -0.6438884357149024,1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719]
pos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384, 1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323]
lpos1 = [-1.4580487388850858, 1.627553615946424, 0.07669903939427068, -0.3539660668045592, -1.9155585088719105, -1.4124128104454947, -0.6438884357149024]
lpos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384]
rpos1 = [1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719]
rpos2 = [1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323]
placegoal = PoseStamped()
placegoal.header.frame_id = "base"
placegoal.header.stamp = rospy.Time.now()
placegoal.pose.position.x = 0.55
placegoal.pose.position.y = 0.28
placegoal.pose.position.z = 0
placegoal.pose.orientation.x = 1.0
placegoal.pose.orientation.y = 0.0
placegoal.pose.orientation.z = 0.0
placegoal.pose.orientation.w = 0.0
# Define variables.
offset_zero_point = 0.903
table_size_x = 0.714655654394
table_size_y = 1.05043717328
table_size_z = 0.729766045265
center_x = 0.457327827197
center_y = 0.145765166941
center_z = -0.538116977368
# The distance from the zero point in Moveit to the ground is 0.903 m.
# The value is not allways the same. (look in Readme)
center_z_cube= -offset_zero_point+table_size_z+0.0275/2
pressure_ok=0
j=0
k=0
start=1
locs_x = []
# Initialize a list for the objects and the stacked cubes.
objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11']
boxlist= ['box01', 'box02', 'box03', 'box04', 'box05', 'box06', 'box07', 'box08', 'box09', 'box10', 'box11']
# Clear planning scene.
p.clear()
# Add table as attached object.
p.attachBox('table', table_size_x, table_size_y, table_size_z, center_x, center_y, center_z, 'base', touch_links=['pedestal'])
# Move both arms to start state where the right camera looks for objects.
g.moveToJointPosition(jts_both, pos1, max_velocity_scaling_factor = 1, plan_only=False)
time.sleep(0.5)
# cProfile to measure the performance (time) of the task.
pr = cProfile.Profile()
pr.enable()
# Loop to continue pick and place until all objects are cleared from table.
while locs_x or start:
# Only for the start.
if start:
start = 0
# Receive the data from all objects from the topic "detected_objects".
temp = rospy.wait_for_message("detected_objects", PoseArray)
locs = temp.poses
locs_x = []
locs_y = []
orien = []
size = []
# Adds the data from the objects.
for i in range(len(locs)):
locs_x.append(locs[i].position.x)
locs_y.append(locs[i].position.y)
orien.append(locs[i].position.z*pi/180)
size.append(locs[i].orientation.x)
# Filter objects list to remove multiple detected locations for same objects.
ind_rmv = []
for i in range(0,len(locs) ):
if (locs_y[i] > 0.24 or locs_x[i] > 0.75):
ind_rmv.append(i)
continue
for j in range(i,len(locs)):
if not (i == j):
if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018:
ind_rmv.append(i)
locs_x = del_meth(locs_x, ind_rmv)
locs_y = del_meth(locs_y, ind_rmv)
orien = del_meth(orien, ind_rmv)
size = del_meth(size, ind_rmv)
# Do the task only if there are still objects on the table.
if locs_x:
# Clear planning scene.
p.clear()
# Add table as attached object.
p.attachBox('table', table_size_x, table_size_y, table_size_z, center_x, center_y, center_z, 'base', touch_links=['pedestal'])
# Sort objects based on size (largest first to smallest last). This was done to enable stacking large cubes.
ig0 = itemgetter(0)
sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0))
locs_x = list(sorted_lists[1])
locs_y = list(sorted_lists[2])
orien = list(sorted_lists[3])
size = list(sorted_lists[0])
# Initialize the data of the biggest object on the table.
xn = locs_x[0]
yn = locs_y[0]
zn = -0.16
thn = orien[0]
sz = size[0]
if thn > pi/4:
thn = -1*(thn%(pi/4))
# Add the detected objects into the planning scene.
#for i in range(1,len(locs_x)):
#p.addBox(objlist[i], 0.05, 0.05, 0.0275, locs_x[i], locs_y[i], center_z_cube)
# Add the stacked objects as collision objects into the planning scene to avoid moving against them.
#for e in range(0, k):
#p.attachBox(boxlist[e], 0.05, 0.05, 0.0275, placegoal.pose.position.x, placegoal.pose.position.y, center_z_cube+0.0275*(e-1), 'base', touch_links=['cubes'])
if k>0:
p.attachBox(boxlist[0], 0.07, 0.07, 0.0275*k, placegoal.pose.position.x, placegoal.pose.position.y, center_z_cube, 'base', touch_links=['cubes'])
p.waitForSync()
# Move both arms to the second position.
g.moveToJointPosition(jts_both, pos2, max_velocity_scaling_factor = 1, plan_only=False)
# Initialize the pickgoal.
pickgoal = PoseStamped()
pickgoal.header.frame_id = "base"
pickgoal.header.stamp = rospy.Time.now()
pickgoal.pose.position.x = xn
pickgoal.pose.position.y = yn
pickgoal.pose.position.z = zn
pickgoal.pose.orientation.x = 1.0
pickgoal.pose.orientation.y = 0.0
pickgoal.pose.orientation.z = 0.0
pickgoal.pose.orientation.w = 0.0
# Move to the approach pickgoal. (5 cm to pickgoal)
pickgoal.pose.position.z = zn+0.05
# Orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles.
pickgoal.pose = rotate_pose_msg_by_euler_angles(pickgoal.pose, 0.0, 0.0, thn)
gl.moveToPose(pickgoal, "left_gripper", max_velocity_scaling_factor = 1, plan_only=False)
pickgoal.pose.position.z = zn
# Move to the pickgoal.
gl.moveToPose(pickgoal, "left_gripper", max_velocity_scaling_factor = 0.3, plan_only=False)
# Read the force in z direction.
b=leftarm.endpoint_effort()
z_= b['force']
z_force= z_.z
# Search again for objects, if the gripper isn't at the right position and presses on an object.
#print("----->force in z direction:", z_force)
if z_force>-4:
leftgripper.close()
attempts=0
pressure_ok=1
# If the gripper hadn't enough pressure after 2 seconds it opens and search again for objects.
while(leftgripper.force()<25 and pressure_ok==1):
time.sleep(0.04)
attempts+=1
if(attempts>50):
leftgripper.open()
pressure_ok=0
print("----->pressure is to low<-----")
else:
print("----->gripper presses on an object<-----")
# Move back to the approach pickgoal.
pickgoal.pose.position.z = zn+0.05
gl.moveToPose(pickgoal, "left_gripper", max_velocity_scaling_factor = 1, plan_only=False)
# Move both arms to position 1.
g.moveToJointPosition(jts_both, pos1, max_velocity_scaling_factor = 1, plan_only=False)
if pressure_ok and z_force>-4:
# Define the approach placegoal.
# Increase the height of the tower every time by 2.75 cm.
placegoal.pose.position.z =-0.145+(k*0.0275)+0.08
# Move to the approach pickgoal.
gl.moveToPose(placegoal, "left_gripper", max_velocity_scaling_factor = 1, plan_only=False)
# Define the placegoal.
placegoal.pose.position.z =-0.145+(k*0.0275)
gl.moveToPose(placegoal, "left_gripper", max_velocity_scaling_factor = 1, plan_only=False)
leftgripper.open()
k += 1
# Move to the approach placegoal.
placegoal.pose.position.z = -0.145+(k*0.0275)+0.08
gl.moveToPose(placegoal, "left_gripper", max_velocity_scaling_factor = 1, plan_only=False)
# Move left arm to start position pos1.
gl.moveToJointPosition(jts_left, lpos1, max_velocity_scaling_factor = 1, plan_only=False)
pr.disable()
sortby = 'cumulative'
ps=pstats.Stats(pr).sort_stats(sortby).print_stats(0.0)
p.clear()
class GraspingClient(GripperCommandClient):
def __init__(self):
# initialize GripperCommandClient
GripperCommandClient.__init__(self)
# initialize Moveit Scene
self.scene = PlanningSceneInterface("base_link")
self.scene2 = moveit_commander.PlanningSceneInterface()
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(
find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
# creat target pose publisher
self.marker_pub = rospy.Publisher("/TargetMarker",
MarkerArray,
queue_size=1)
# creat basket pos publisher
self.marker_pub2 = rospy.Publisher("/basket",
MarkerArray,
queue_size=1)
# creat publisher for requesting grasp pose
self.request_cloud_client = rospy.Publisher("/request_cloud",
Int32,
queue_size=1)
# instantiate a RobotCommander
self.robot = moveit_commander.RobotCommander()
# instantiate two MoveGroupCommander
self.group = moveit_commander.MoveGroupCommander("arm")
# basket's parameters
self.basket_pos = 'left'
self.basket_found = False
self.basket_pos_x = 0
self.basket_pos_y = 0
self.basket_search_t = 0
def updateScene(self):
# detect objects
goal = FindGraspableObjectsGoal()
goal.plan_grasps = False
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
find_result = self.find_client.get_result()
#remove original objects
self.clear_scene()
# all the objects in scene
self.objects = list()
# height is used for grasping selection
self.height = -1
# object number
object_num = -1
# add support surface to scene
for obj in find_result.support_surfaces:
# extend surface to floor
h = obj.primitive_poses[0].position.z
if (h + obj.primitives[0].dimensions[2] / 2.0) > self.height:
self.height = h + obj.primitives[0].dimensions[2] / 2.0
self.table = obj
obj.primitives[0].dimensions = [
obj.primitives[0].dimensions[0] + 0.02,
obj.primitives[0].dimensions[1] + 0.02,
obj.primitives[0].dimensions[2] + h - 0.02
]
obj.primitive_poses[0].position.z += -h / 2.0
# add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
wait=False)
# add objects Solid to scene
for obj in find_result.objects:
object_num += 1
obj.object.name = "object%d" % object_num
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
wait=False)
if obj.object.primitive_poses[0].position.z > self.height:
self.objects.append([
obj,
numpy.array([
obj.object.primitive_poses[0].position.x,
obj.object.primitive_poses[0].position.y,
obj.object.primitive_poses[0].position.z
]), obj.object.primitive_poses[0].position.x
])
# localize basket
if obj.object.primitive_poses[0].position.y > self.table.primitive_poses[0].position.y + self.table.primitives[0].dimensions[1] / 2.0 + 0.1 or \
obj.object.primitive_poses[0].position.y < self.table.primitive_poses[0].position.y - self.table.primitives[0].dimensions[1] / 2.0 - 0.1:
if obj.object.primitives[0].dimensions[
1] > 0.20 and obj.object.primitive_poses[
0].position.z > 0.3 and not self.basket_found:
if self.basket_search_t == 1:
self.basket_pos_x = obj.object.primitive_poses[
0].position.x
self.basket_pos_y = obj.object.primitive_poses[
0].position.y
self.basket_search_t += 1
elif self.basket_search_t == 2:
if self.basket_pos_x - obj.object.primitive_poses[0].position.x < 0.04 and \
self.basket_pos_y - obj.object.primitive_poses[0].position.y < 0.04:
self.basket_found = True
if obj.object.primitive_poses[0].position.y < 0:
self.basket_pos = 'right'
self.basket = obj
self.marker_pub2.publish(self.basket_marker())
rospy.loginfo("Basket is found at %s..." %
self.basket_pos)
else:
self.basket_search_t -= 1
if self.basket_search_t == 0:
self.basket_search_t += 1
self.scene.waitForSync()
def clear_scene(self):
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()
def clear_target_object(self, pose):
if len(self.objects) == 1:
self.scene.removeCollisionObject(self.objects[0][0].object.name)
self.tname = self.objects[0][0].object.name
else:
dist = float('inf')
vec = numpy.array([
pose[0].center.x + 0.33 * pose[0].approach.x,
pose[0].center.y + 0.33 * pose[0].approach.y,
pose[0].center.z + 0.33 * pose[0].approach.z
])
for obj in self.objects:
temp = numpy.linalg.norm(obj[1] - vec)
if temp < dist:
dist = temp
self.tname = obj[0].object.name
self.scene.removeCollisionObject(self.tname, False)
def readd_target_object(self):
for obj in self.objects:
if obj[0].object.name == self.tname:
self.scene.addSolidPrimitive(obj[0].object.name,
obj[0].object.primitives[0],
obj[0].object.primitive_poses[0],
wait=False)
def request_grasp_poses(self, mode=None):
# possible grasp poses
self.grasp_pose = list()
rospy.wait_for_service('grasp_pose')
res = rospy.ServiceProxy('grasp_pose', grasp_pose)
grasps_obtained = False
while not grasps_obtained:
try:
self.grasp_pose[:] = []
rospy.loginfo("Try to find feasible grasp poses ...")
# request PointCloud data for agile grasp package
self.request_cloud_client.publish(1)
grasps = res()
if mode == 'cluster':
self.pose_selection_cluster(grasps.grasps)
else:
self.pose_selection_general(grasps.grasps)
if len(self.grasp_pose) > 0:
self.TargetGraspPoses, self.Rots = self.creat_target_grasp_pose(
self.grasp_pose)
if self.TargetGraspPoses != None:
grasps_obtained = True
except rospy.ServiceException, e:
rospy.loginfo("Service call failed: %s" % e)
continue
# box_pose = geometry_msgs.msg.PoseStamped()
# box_pose.header.frame_id = robot.get_planning_frame()
# box_pose.pose.position.x = 1.
# box_pose.pose.position.y = 0.5
# box_pose.pose.position.z = 0.5
# scene.add_box("table", box_pose, (0.5, 1.5, 0.6))
# name size and position
p.addBox("first part", 0.8, 0.43, 0.04, 1.23, 0, 0.36)
p.addBox("under part", 0.8, 0.43, 0.04, 1.23, 0, 0.0)
p.addBox("second part", 0.8, 0.01, 1.6, 1.23, -0.22, 0)
p.addBox("third part", 0.8, 0.01, 1.6, 1.23, 0.22, 0)
p.waitForSync()
def load_gazebo_models(table_pose=Pose(position=Point(x=0.7, y=0.0, z=0.0)),
table_reference_frame="world",
block_pose1=Pose(position=Point(x=0.6725, y=0.0765, z=-0.135)),
block_pose2=Pose(position=Point(x=0.55, y=-0.2, z=-0.135)),
block_pose3=Pose(position=Point(x=0.7, y=-0.1, z=-0.135)),
block_pose4=Pose(position=Point(x=0.58, y=-0.03, z=-0.135)),
block_reference_frame="base"):
pass
# Get Models' Path.
model_path = rospkg.RosPack().get_path('baxter_pnp_one_arm_cartesian_sim')+"/models/"
# Load Table SDF.
table_xml = ''
with open (model_path + "cafe_table/model.sdf", "r") as table_file:
table_xml=table_file.read().replace('\n', '')
if __name__ == "__main__":
parser = argparse.ArgumentParser(
description="Remove objects from the MoveIt planning scene.")
parser.add_argument("name", nargs="?", help="Name of object to remove")
parser.add_argument("--all",
help="Remove all objects.",
action="store_true")
parser.add_argument("--attached",
help="Remove an attached object.",
action="store_true")
args = parser.parse_args()
if args.all:
rospy.init_node("remove_objects")
scene = PlanningSceneInterface("base_link")
for name in scene.getKnownCollisionObjects():
print("Removing %s" % name)
scene.removeCollisionObject(name, use_service=False)
scene.waitForSync()
elif args.name:
rospy.init_node("remove_objects")
scene = PlanningSceneInterface("base_link")
print("Removing %s" % args.name)
if args.attached:
scene.removeAttachedObject(args.name)
else:
scene.removeCollisionObject(args.name)
else:
parser.print_help()
def clasificar():
adaptative = True
pr_b = False
precision = 0.7
# Can free memory?
pub = rospy.Publisher("finished", String, queue_size=10)
# Initialize MoveIt! scene
p = PlanningSceneInterface("base")
arms_group = MoveGroupInterface("both_arms", "base")
rightarm_group = MoveGroupInterface("right_arm", "base")
leftarm_group = MoveGroupInterface("left_arm", "base")
# Create right arm instance
right_arm = baxter_interface.limb.Limb('right')
# Create right gripper instance
right_gripper = baxter_interface.Gripper('right')
right_gripper.calibrate()
right_gripper.open()
right_gripper.close()
offset_zero_point = 0.903
table_size_x = 0.714655654394
table_size_y = 1.05043717328
table_size_z = 0.729766045265
center_x = 0.457327827197
center_y = 0.145765166941
center_z = -0.538116977368
table_distance_from_gripper = -offset_zero_point + table_size_z + 0.0275 / 2
j = 0
k = 0
# Initialize object list
objlist = [
'obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08',
'obj09', 'obj10', 'obj11'
]
p.clear()
p.attachBox('table',
table_size_x,
table_size_y,
table_size_z,
center_x,
center_y,
center_z,
'base',
touch_links=['pedestal'])
p.waitForSync()
# Move both arms to start state.
# Initial pos
rpos = PoseStamped()
rpos.header.frame_id = "base"
rpos.header.stamp = rospy.Time.now()
rpos.pose.position.x = 0.555
rpos.pose.position.y = 0.0
rpos.pose.position.z = 0.206
rpos.pose.orientation.x = 1.0
rpos.pose.orientation.y = 0.0
rpos.pose.orientation.z = 0.0
rpos.pose.orientation.w = 0.0
lpos = PoseStamped()
lpos.header.frame_id = "base"
lpos.header.stamp = rospy.Time.now()
lpos.pose.position.x = 0.65
lpos.pose.position.y = 0.6
lpos.pose.position.z = 0.206
lpos.pose.orientation.x = 1.0
lpos.pose.orientation.y = 0.0
lpos.pose.orientation.z = 0.0
lpos.pose.orientation.w = 0.0
while True:
# Move to initial position
rightarm_group.moveToPose(rpos,
"right_gripper",
max_velocity_scaling_factor=1,
plan_only=False)
leftarm_group.moveToPose(lpos,
"left_gripper",
max_velocity_scaling_factor=1,
plan_only=False)
# Get the middle point
locs = PoseArray()
locs = rospy.wait_for_message("clasificacion", PoseArray)
if (len(locs.poses) != 0):
punto_medio = PoseStamped()
punto_medio.header.frame_id = "base"
punto_medio.header.stamp = rospy.Time.now()
punto_medio.pose.position.x = locs.poses[0].position.x
punto_medio.pose.position.y = locs.poses[0].position.y
punto_medio.pose.position.z = locs.poses[0].position.z
punto_medio.pose.orientation.x = locs.poses[0].orientation.x
punto_medio.pose.orientation.y = locs.poses[0].orientation.y
punto_medio.pose.orientation.z = locs.poses[0].orientation.z
punto_medio.pose.orientation.w = locs.poses[0].orientation.w
alfa = 0.1
# Two parameters:
# When pr_b == 1, the program will try to adapt the trajectory for getting the precision established in the parameter of the same name
# When adaptative == 1, the program will try to get the best precision based on the precision of the last execution.
if (pr_b and not adaptative):
if precision >= 1:
punto_medio.pose.position.x += 0.01
else:
punto_medio.pose.position.x -= alfa * (1 - precision)
else:
print("If it is the first time executing, write -1")
precision_value = input()
if precision_value != -1.0:
punto_medio.pose.position.x += alfa * (1 - precision_value)
elif precision_value == 1.0:
adaptative = False
# Get the normal orientation for separating the objects
orient = (-1) * (1 / punto_medio.pose.orientation.x)
punto_medio.pose = rotate_pose_msg_by_euler_angles(
punto_medio.pose, 0.0, 0.0, orient)
rightarm_group.moveToPose(punto_medio,
"right_gripper",
max_velocity_scaling_factor=1,
plan_only=False)
orient = 1.57 - orient
punto_medio.pose = rotate_pose_msg_by_euler_angles(
punto_medio.pose, 0.0, 0.0, orient)
rightarm_group.moveToPose(punto_medio,
"right_gripper",
max_velocity_scaling_factor=1,
plan_only=False)
punto_medio.pose.position.x += 0.15
rightarm_group.moveToPose(punto_medio,
"right_gripper",
max_velocity_scaling_factor=1,
plan_only=False)
time.sleep(1)
punto_medio.pose.position.x -= 0.3
rightarm_group.moveToPose(punto_medio,
"right_gripper",
max_velocity_scaling_factor=1,
plan_only=False)
time.sleep(1)
rightarm_group.moveToPose(rpos,
"right_gripper",
max_velocity_scaling_factor=1,
plan_only=False)
# Free the memory when finished
freemem = "free"
if not adaptative:
pub.publish(freemem)
else:
sys.exit("Cannot identify any object")
class GraspingClient(object):
def __init__(self, sim=False):
self.scene = PlanningSceneInterface("base_link")
self.dof = rospy.get_param('~jaco_dof')
self.move_group = MoveGroupInterface("upper_body", "base_link")
self.lmove_group = MoveGroupInterface("left_arm", "base_link")
self.rmove_group = MoveGroupInterface("right_arm", "base_link")
self.move_group.setPlannerId("RRTConnectkConfigDefault")
self.lmove_group.setPlannerId("RRTConnectkConfigDefault")
self.rmove_group.setPlannerId("RRTConnectkConfigDefault")
if "6dof" == self.dof:
self._upper_body_joints = [
"right_elbow_joint", "right_shoulder_lift_joint",
"right_shoulder_pan_joint", "right_wrist_1_joint",
"right_wrist_2_joint", "right_wrist_3_joint",
"left_elbow_joint", "left_shoulder_lift_joint",
"left_shoulder_pan_joint", "left_wrist_1_joint",
"left_wrist_2_joint", "left_wrist_3_joint", "linear_joint",
"pan_joint", "tilt_joint"
]
self._right_arm_joints = [
"right_elbow_joint", "right_shoulder_lift_joint",
"right_shoulder_pan_joint", "right_wrist_1_joint",
"right_wrist_2_joint", "right_wrist_3_joint"
]
self._left_arm_joints = [
"left_elbow_joint", "left_shoulder_lift_joint",
"left_shoulder_pan_joint", "left_wrist_1_joint",
"left_wrist_2_joint", "left_wrist_3_joint"
]
self.tucked = [
-2.8, -1.48, -1.48, 0, 0, 1.571, 2.8, 1.48, 1.48, 0, 0, -1.571,
0.0371, 0.0, 0.0
]
self.constrained_stow = [
2.28, 2.17, -2.56, -0.09, 0.15, 1.082, -2.28, -2.17, 2.56,
0.09, -0.15, 2.06, 0.42, 0.0, 0.0
]
self.larm_const_stow = [-2.28, -2.17, 2.56, 0.09, -0.15, 2.08]
self.rarm_const_stow = [2.28, 2.17, -2.56, -0.09, 0.15, 1.06]
self.tableDist = 0.7
elif "7dof" == self.dof:
self._upper_body_joints = [
"right_shoulder_pan_joint", "right_shoulder_lift_joint",
"right_arm_half_joint", "right_elbow_joint",
"right_wrist_spherical_1_joint",
"right_wrist_spherical_2_joint", "right_wrist_3_joint",
"left_shoulder_pan_joint", "left_shoulder_lift_joint",
"left_arm_half_joint", "left_elbow_joint",
"left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint",
"left_wrist_3_joint", "linear_joint", "pan_joint", "tilt_joint"
]
self._right_arm_joints = [
"right_shoulder_pan_joint", "right_shoulder_lift_joint",
"right_arm_half_joint", "right_elbow_joint",
"right_wrist_spherical_1_joint",
"right_wrist_spherical_2_joint", "right_wrist_3_joint"
]
self._left_arm_joints = [
"left_shoulder_pan_joint", "left_shoulder_lift_joint",
"left_arm_half_joint", "left_elbow_joint",
"left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint",
"left_wrist_3_joint"
]
self.tucked = [
-1.6, -1.5, 0.4, -2.7, 0.0, 0.5, -1.7, 1.6, 1.5, -0.4, 2.7,
0.0, -0.5, 1.7, 0.04, 0, 0
]
self.constrained_stow = [
-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, 1.0, 2.6, -2.0, 0.0, -2.0, 0.0,
0.0, -1.0, 0.42, 0, 0
]
self.larm_const_stow = [2.6, -2.0, 0.0, -2.0, 0.0, 0.0, 1.0]
self.rarm_const_stow = [-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, -1.0]
self.tableDist = 0.8
else:
rospy.logerr("DoF needs to be set 6 or 7, aborting demo")
return
self.pickplace = [None] * 2
self.pickplace[0] = PickPlaceInterface("left_side",
"left_gripper",
verbose=True)
self.pickplace[0].planner_id = "RRTConnectkConfigDefault"
self.pickplace[1] = PickPlaceInterface("right_side",
"right_gripper",
verbose=True)
self.pickplace[1].planner_id = "RRTConnectkConfigDefault"
self.pick_result = [None] * 2
self._last_gripper_picked = 0
self.place_result = [None] * 2
self._last_gripper_placed = 0
self._objs_to_keep = []
self._listener = tf.TransformListener()
self._lgripper = GripperActionClient('left')
self._rgripper = GripperActionClient('right')
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(
find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
self.scene.clear()
# This is a simulation so need to adjust gripper parameters
if sim:
self._gripper_closed = 0.96
self._gripper_open = 0.00
else:
self._gripper_closed = 0.01
self._gripper_open = 0.165
def add_objects_to_keep(self, obj):
self._objs_to_keep.append(obj)
def clearScene(self):
self.scene.clear()
def getPickCoordinates(self):
self.updateScene(0, False)
beer, grasps = self.getGraspableBeer(False)
pringles, grasps = self.getGraspablePringles(False)
if (None == beer) or (None == pringles):
return None
center_objects = (beer.primitive_poses[0].position.y +
pringles.primitive_poses[0].position.y) / 2
surface = self.getSupportSurface(beer.support_surface)
tmp1 = surface.primitive_poses[
0].position.x - surface.primitives[0].dimensions[0] / 2
surface = self.getSupportSurface(pringles.support_surface)
tmp2 = surface.primitive_poses[
0].position.x - surface.primitives[0].dimensions[0] / 2
front_edge = (tmp1 + tmp2) / 2
coords = Pose2D(x=(front_edge - self.tableDist),
y=center_objects,
theta=0.0)
return coords
def updateScene(self, gripper=0, plan=True):
# find objects
rospy.loginfo("Updating scene...")
goal = FindGraspableObjectsGoal()
goal.plan_grasps = plan
goal.gripper = gripper
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
find_result = self.find_client.get_result()
# remove previous objects
for name in self.scene.getKnownCollisionObjects():
self.scene.removeCollisionObject(name, True)
# insert objects to scene
idx = -1
for obj in find_result.objects:
if obj.object.primitive_poses[
0].position.z < 0.5 or obj.object.primitive_poses[
0].position.x > 2.0 or obj.object.primitive_poses[
0].position.y > 0.5:
continue
idx += 1
obj.object.name = "object%d_%d" % (idx, gripper)
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
wait=True)
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
if obj.primitive_poses[0].position.z < 0.5:
continue
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [
obj.primitives[0].dimensions[0],
obj.primitives[0].dimensions[1], # wider
obj.primitives[0].dimensions[2] + height
]
obj.primitive_poses[0].position.z += -height / 2.0
# add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
wait=True)
self.scene.waitForSync()
# store for grasping
self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
def getGraspableBeer(self, planned=True):
for obj in self.objects:
# need grasps
if len(obj.grasps) < 1 and planned:
continue
# has to be on table
if obj.object.primitive_poses[0].position.z < 0.5 or \
obj.object.primitive_poses[0].position.z > 1.0 or \
obj.object.primitive_poses[0].position.x > 2.0:
continue
elif (obj.object.primitives[0].type == sp.CYLINDER):
if obj.object.primitives[0].dimensions[sp.CYLINDER_HEIGHT] < 0.102 or \
obj.object.primitives[0].dimensions[sp.CYLINDER_HEIGHT] > 0.142:
continue
elif (obj.object.primitives[0].type == sp.BOX):
if obj.object.primitives[0].dimensions[sp.BOX_Z] < 0.102 or \
obj.object.primitives[0].dimensions[sp.BOX_Z] > 0.142:
continue
else:
continue
print "beer: ", obj.object.primitive_poses[0]
return obj.object, obj.grasps
# nothing detected
return None, None
def getGraspablePringles(self, planned=True):
for obj in self.objects:
# need grasps
if len(obj.grasps) < 1 and planned:
continue
# has to be on table
if obj.object.primitive_poses[0].position.z < 0.5 or \
obj.object.primitive_poses[0].position.z > 1.0 or \
obj.object.primitive_poses[0].position.x > 2.0:
continue
elif (obj.object.primitives[0].type == sp.CYLINDER):
if obj.object.primitives[0].dimensions[sp.CYLINDER_HEIGHT] < 0.21 or \
obj.object.primitives[0].dimensions[sp.CYLINDER_HEIGHT] > 0.28:
continue
elif (obj.object.primitives[0].type == sp.BOX):
if obj.object.primitives[0].dimensions[sp.BOX_Z] < 0.21 or \
obj.object.primitives[0].dimensions[sp.BOX_Z] > 0.28:
continue
else:
continue
return obj.object, obj.grasps
# nothing detected
return None, None
def getSupportSurface(self, name):
for surface in self.surfaces:
if surface.name == name:
return surface
return None
def getPlaceLocation(self):
pass
def pick(self, block, grasps, gripper=0):
success, pick_result = self.pickplace[gripper].pick_with_retry(
block.name,
grasps,
retries=1,
support_name=block.support_surface,
scene=self.scene)
self.pick_result[gripper] = pick_result
self._last_gripper_picked = gripper
return success
def place(self, block, pose_stamped, gripper=0):
places = list()
l = PlaceLocation()
l.place_pose.pose = pose_stamped.pose
l.place_pose.header.frame_id = pose_stamped.header.frame_id
# copy the posture, approach and retreat from the grasp used
l.post_place_posture = self.pick_result[
gripper].grasp.pre_grasp_posture
l.pre_place_approach = self.pick_result[
gripper].grasp.pre_grasp_approach
l.post_place_retreat = self.pick_result[
gripper].grasp.post_grasp_retreat
places.append(copy.deepcopy(l))
# create another several places, rotate each by 360/m degrees in yaw direction
m = 16 # number of possible place poses
pi = 3.141592653589
for i in range(0, m - 1):
l.place_pose.pose = rotate_pose_msg_by_euler_angles(
l.place_pose.pose, 0, 0, 2 * pi / m)
places.append(copy.deepcopy(l))
success, place_result = self.pickplace[gripper].place_with_retry(
block.name, places, retries=1, scene=self.scene)
self.place_result[gripper] = place_result
self._last_gripper_placed = gripper
return success
def goto_tuck(self):
# remove previous objects
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(
self._upper_body_joints, self.tucked, 0.05)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def goto_plan_grasp(self):
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(
self._upper_body_joints, self.constrained_stow, 0.05)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def left_arm_constrained_stow(self):
c1 = Constraints()
c1.orientation_constraints.append(OrientationConstraint())
c1.orientation_constraints[0].header.stamp = rospy.get_rostime()
c1.orientation_constraints[0].header.frame_id = "base_link"
c1.orientation_constraints[0].link_name = "left_ee_link"
c1.orientation_constraints[0].orientation.w = 1.0
c1.orientation_constraints[
0].absolute_x_axis_tolerance = 0.2 #x axis is pointed up for wrist link
c1.orientation_constraints[0].absolute_y_axis_tolerance = 0.2
c1.orientation_constraints[0].absolute_z_axis_tolerance = 6.28
c1.orientation_constraints[0].weight = 1.0
while not rospy.is_shutdown():
result = self.lmove_group.moveToJointPosition(
self._left_arm_joints,
self.larm_const_stow,
0.05,
path_constraints=c1,
planning_time=120.0)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def right_arm_constrained_stow(self):
c1 = Constraints()
c1.orientation_constraints.append(OrientationConstraint())
c1.orientation_constraints[0].header.stamp = rospy.get_rostime()
c1.orientation_constraints[0].header.frame_id = "base_link"
c1.orientation_constraints[0].link_name = "right_ee_link"
c1.orientation_constraints[0].orientation.w = 1.0
c1.orientation_constraints[
0].absolute_x_axis_tolerance = 0.2 #x axis is pointed up for wrist link
c1.orientation_constraints[0].absolute_y_axis_tolerance = 0.2
c1.orientation_constraints[0].absolute_z_axis_tolerance = 6.28
c1.orientation_constraints[0].weight = 1.0
while not rospy.is_shutdown():
result = self.rmove_group.moveToJointPosition(
self._right_arm_joints,
self.rarm_const_stow,
0.05,
path_constraints=c1,
planning_time=120.0)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def open_gripper(self):
self._lgripper.command(self._gripper_open, block=True)
self._rgripper.command(self._gripper_open, block=True)
def close_gripper(self):
self._lgripper.command(self._gripper_closed, block=True)
self._rgripper.command(self._gripper_closed, block=True)
description="Remove objects from the MoveIt planning scene.")
parser.add_argument("name",
nargs="?",
help="Name of object to remove")
parser.add_argument("--all",
help="Remove all objects.",
action="store_true")
parser.add_argument("--attached",
help="Remove an attached object.",
action="store_true")
args = parser.parse_args()
if args.all:
rospy.init_node("remove_objects")
scene = PlanningSceneInterface("base_link")
for name in scene.getKnownCollisionObjects():
print("Removing %s" % name)
scene.removeCollisionObject(name, use_service=False)
scene.waitForSync()
elif args.name:
rospy.init_node("remove_objects")
scene = PlanningSceneInterface("base_link")
print("Removing %s" % args.name)
if args.attached:
scene.removeAttachedObject(args.name)
else:
scene.removeCollisionObject(args.name)
else:
parser.print_help()
class GripperInterfaceClient():
def __init__(self):
self.joint_names = ["l_gripper_finger_joint", "r_gripper_finger_joint"]
self.client = actionlib.SimpleActionClient("gripper_controller/gripper_action",
GripperCommandAction)
rospy.loginfo("Waiting for gripper_controller...")
self.client.wait_for_server()
rospy.loginfo("...connected.")
self.scene = PlanningSceneInterface("base_link")
self.attached_object_publisher = rospy.Publisher('attached_collision_object',
AttachedCollisionObject,
queue_size = 10)
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True, plan_only=True)
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
self.debug_index = -1
self.graspables = []
def execute_trajectory(self, trajectory):
goal_position = trajectory.points[-1].positions
print '==========', goal_position
command_goal = GripperCommandGoal()
command_goal.command.position = goal_position[0]+goal_position[1]
command_goal.command.max_effort = 50.0 # Placeholder. TODO Figure out a better way to compute this
self.client.send_goal(command_goal)
self.client.wait_for_result()
def move_to(self, goal_position, max_effort):
command_goal = GripperCommandGoal()
command_goal.command.position = goal_position
command_goal.command.max_effort = max_effort
self.client.send_goal(command_goal)
self.client.wait_for_result()
def updateScene(self):
# find objects
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
find_result = self.find_client.get_result()
# remove previous objects
for name in self.scene.getKnownCollisionObjects():
self.scene.removeCollisionObject(name, wait=False)
for name in self.scene.getKnownAttachedObjects():
self.scene.removeAttachedObject(name, wait=False)
rospy.sleep(0.5) # Gets rid of annoying error messages stemming from race condition.
self.scene.waitForSync()
# insert objects to scene
idx = -1
for obj in find_result.objects:
idx += 1
obj.object.name = "object%d"%idx
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
wait = False)
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0] + 0.1,
1.5, # wider
obj.primitives[0].dimensions[2] + height]
obj.primitive_poses[0].position.z += -height/2.0
# add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
wait = False)
self.scene.waitForSync()
# store for grasping
self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
def getGraspableCube(self):
graspable = None
for obj in self.objects:
# need grasps
if len(obj.grasps) < 1:
continue
# check size
#if obj.object.primitives[0].dimensions[0] < 0.05 or \
# obj.object.primitives[0].dimensions[0] > 0.07 or \
# obj.object.primitives[0].dimensions[0] < 0.05 or \
# obj.object.primitives[0].dimensions[0] > 0.07 or \
# obj.object.primitives[0].dimensions[0] < 0.05 or \
# obj.object.primitives[0].dimensions[0] > 0.07:
# continue
# has to be on table
#if obj.object.primitive_poses[0].position.z < 0.5:
# continue
return obj.object, obj.grasps
# nothing detected
return None, None
def set_target_object(self, primitive_type, target_pose, dimensions):
primitive_types = {"BOX":1, "SPHERE":2, "CYLINDER":3, "CONE":4}
self.sought_object = Object()
primitive = SolidPrimitive()
primitive.type = primitive_types[primitive_type.upper()]
primitive.dimensions = dimensions
self.sought_object.primitives.append(primitive)
p = Pose()
p.position.x = target_pose[0]
p.position.y = target_pose[1]
p.position.z = target_pose[2]
quaternion = quaternion_from_euler(target_pose[3], target_pose[4], target_pose[5])
p.orientation.x = quaternion[0]
p.orientation.y = quaternion[1]
p.orientation.z = quaternion[2]
p.orientation.w = quaternion[3]
self.sought_object.primitive_poses.append(p)
def find_graspable_object(self, tolerance=0.01):
self.updateScene()
self.current_grasping_target = None
for obj in self.objects:
# Must have grasps
if len(obj.grasps) < 1:
continue
# Compare to sought object
detected_object_dimensions = np.array(obj.object.primitives[0].dimensions)
sought_object_dimensions = np.array(self.sought_object.primitives[0].dimensions)
try:
difference = detected_object_dimensions - sought_object_dimensions
print "===DEBUG: Difference: " , difference
mag_diff = np.linalg.norm(difference)
print "===DEBUG: mag_diff: ", mag_diff
if mag_diff <= tolerance:
self.current_grasping_target = obj
tolerance = mag_diff
print "===DEBUG: Tolerance: ", tolerance
else:
rospy.loginfo("Object dimensions do not match. Trying another object...")
except:
rospy.loginfo("Object geometries do not match. Trying another object...")
if self.current_grasping_target is None:
tolerance += .01
self.find_graspable_object(tolerance = tolerance)
# Nothing detected
def computeGraspToPickMatrix(self):
pick_rot = self.current_grasping_target.object.primitive_poses[0].orientation
grasp_rot = self.pick_result.grasp.grasp_pose.pose.orientation
pick_translation_distance = self.pick_result.grasp.pre_grasp_approach.desired_distance
pick_quaternion = [pick_rot.x, pick_rot.y, pick_rot.z, pick_rot.w]
grasp_quaternion = [grasp_rot.x, grasp_rot.y, grasp_rot.z, grasp_rot.w]
pick_to_grasp_quaternion = quaternion_multiply(quaternion_inverse(grasp_quaternion), pick_quaternion)
rotation_matrix = quaternion_matrix(pick_to_grasp_quaternion)
translation = [pick_translation_distance, 0, 0]
rotation_matrix[[0,1,2],3] = translation
pick_to_grasp_matrix = np.mat(rotation_matrix)
grasp_to_pick_matrix = pick_to_grasp_matrix.getI()
return grasp_to_pick_matrix
def computePlaceToBaseMatrix(self, place):
place_quaternion = place[3:]
rotation_matrix = quaternion_matrix(place_quaternion)
translation = place[0:3]
rotation_matrix[[0,1,2], 3] = translation
place_to_base_matrix = rotation_matrix
return place_to_base_matrix
def broadcastCurrentGraspingTargetTransform(self):
pose = self.current_grasping_target.object.primitive_poses[0]
br = tf2_ros.TransformBroadcaster()
t = geometry_msgs.msg.TransformStamped()
t.header.stamp = rospy.Time.now()
t.header.frame_id = "base_link"
t.child_frame_id = "current_grasping_target"
t.transform.translation.x = pose.position.x
t.transform.translation.y = pose.position.y
t.transform.translation.z = pose.position.z
t.transform.rotation.x = pose.orientation.x
t.transform.rotation.y = pose.orientation.y
t.transform.rotation.z = pose.orientation.z
t.transform.rotation.w = pose.orientation.w
br.sendTransform(t)
def plan_pick(self):
success, pick_result = self.pickplace.pick_with_retry(self.current_grasping_target.object.name,
self.current_grasping_target.grasps,
support_name = self.current_grasping_target.object.support_surface,
scene=self.scene,
allow_gripper_support_collision=False,
planner_id='PRMkConfigDefault')
self.pick_result = pick_result
print "DEBUG: plan_pick(): ", self.scene.getKnownAttachedObjects()
if success:
rospy.loginfo("Planning succeeded.")
trajectory_approved = get_user_approval()
if trajectory_approved:
return pick_result.trajectory_stages
else:
rospy.loginfo("Plan rejected. Getting new grasps for replan...")
else:
rospy.logwarn("Planning failed. Getting new grasps for replan...")
self.find_graspable_object()
return self.plan_pick()
def plan_place(self, desired_pose):
places = list()
ps = PoseStamped()
#ps.pose = self.current_grasping_target.object.primitive_poses[0]
ps.header.frame_id = self.current_grasping_target.object.header.frame_id
grasp_to_pick_matrix = self.computeGraspToPickMatrix()
place_to_base_matrix = self.computePlaceToBaseMatrix(desired_pose)
grasp2_to_place_matrix = place_to_base_matrix * grasp_to_pick_matrix
position_vector = grasp2_to_place_matrix[0:-1,3]
quaternion = quaternion_from_matrix(grasp2_to_place_matrix)
position_array = position_vector.getA1()
l = PlaceLocation()
direction_components = self.pick_result.grasp.pre_grasp_approach.direction.vector
l.place_pose.header.frame_id = ps.header.frame_id
l.place_pose.pose.position.x = position_array[0]
l.place_pose.pose.position.y = position_array[1]
l.place_pose.pose.position.z = position_array[2]
l.place_pose.pose.orientation.x = quaternion[0]
l.place_pose.pose.orientation.y = quaternion[1]
l.place_pose.pose.orientation.z = quaternion[2]
l.place_pose.pose.orientation.w = quaternion[3]
# copy the posture, approach and retreat from the grasp used
approach = copy.deepcopy(self.pick_result.grasp.pre_grasp_approach)
approach.desired_distance /= 2.0
l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
l.pre_place_approach = approach
l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
places.append(copy.deepcopy(l))
# create another several places, rotate each by 90 degrees in roll direction
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0)
places.append(copy.deepcopy(l))
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0)
places.append(copy.deepcopy(l))
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0)
places.append(copy.deepcopy(l))
print "DEBUG: Places: ", places[0]
success, place_result = self.pickplace.place_with_retry(self.current_grasping_target.object.name,
places,
scene=self.scene,
allow_gripper_support_collision=False,
planner_id='PRMkConfigDefault')
if success:
rospy.loginfo("Planning succeeded.")
trajectory_approved = get_user_approval()
if trajectory_approved:
return place_result.trajectory_stages
else:
rospy.loginfo("Plan rejected. Replanning...")
else:
rospy.logwarn("Planning failed. Replanning...")
desired_pose[2] += 0.05
return self.plan_place(desired_pose)
def recognizeAttachedObject(self):
print "========= Recognizing attached object"
name = self.current_grasping_target.object.name
size_x = self.current_grasping_target.object.primitives[0].dimensions[0]
size_y = self.current_grasping_target.object.primitives[0].dimensions[1]
size_z = self.current_grasping_target.object.primitives[0].dimensions[2]
(x, y, z) = (0.03, 0.0, 0.0)
link_name = "gripper_link"
touch_links = ["l_gripper_finger_link", "r_gripper_finger_link", "gripper_link"]
detach_posture = None
weight = 0.0
wait = True
"""
object_x = self.current_grasping_target.object.primitive_poses[0].position.x
object_y = self.current_grasping_target.object.primitive_poses[0].position.y
object_z = self.current_grasping_target.object.primitive_poses[0].position.z
pick_rot = self.current_grasping_target.object.primitive_poses[0].orientation
grasp_rot = self.pick_result.grasp.grasp_pose.pose.orientation
grasp_position = self.pick_result.grasp.grasp_pose.pose.position
pick_translation_distance = self.pick_result.grasp.post_grasp_retreat.desired_distance
pick_quaternion = [pick_rot.x, pick_rot.y, pick_rot.z, pick_rot.w]
grasp_quaternion = [grasp_rot.x, grasp_rot.y, grasp_rot.z, grasp_rot.w]
pick_to_grasp_quaternion = quaternion_multiply(quaternion_inverse(grasp_quaternion), pick_quaternion)
grasp_to_base_matrix_array = quaternion_matrix(grasp_quaternion)
grasp_to_base_matrix = np.matrix(grasp_to_base_matrix_array)
displacement = grasp_to_base_matrix.dot(np.matrix([[pick_translation_distance-.03], [0], [0], [0]]))
print displacement
attached_location = list()
attached_location.append(object_x - displacement[0,0])
attached_location.append(object_y - displacement[1,0])
attached_location.append(object_z - displacement[2,0])
print attached_location
quaternion = Quaternion()
quaternion.x = pick_to_grasp_quaternion[0]
quaternion.y = pick_to_grasp_quaternion[1]
quaternion.z = pick_to_grasp_quaternion[2]
quaternion.w = pick_to_grasp_quaternion[3]
pose = Pose()
pose.position.x = attached_location[0]
pose.position.y = attached_location[1]
pose.position.z = attached_location[2]
pose.orientation = self.current_grasping_target.object.primitive_poses[0].orientation
collision_object = self.scene.makeSolidPrimitive(self.current_grasping_target.object.name,
self.current_grasping_target.object.primitives[0],
pose)
attached_collision_object = self.scene.makeAttached("gripper_link",
collision_object,
touch_links, detach_posture, 0.0)
self.attached_object_publisher.publish(attached_collision_object)
"""
self.scene.attachBox(name, size_x, size_y, size_z, x, y, z, link_name,
touch_links=touch_links, detach_posture=detach_posture, weight=weight,
wait=wait)
def removeCollisionObjects(self):
collision_object_names = self.scene.getKnownCollisionObjects()
print self.current_grasping_target.object.name
print collision_object_names
raw_input("press ENTER")
x = self.scene.getKnownAttachedObjects()
print x
raw_input("press Enter")
for name in collision_object_names:
if name != self.current_grasping_target.object.name:
self.scene.removeCollisionObject(name, wait=False)
class GraspingClient(object):
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("left_arm", "left_hand", verbose = True)
self.move_group = MoveGroupInterface("left_arm", "base_link")
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server(rospy.Duration(15.0))
self.cubes = []
self.tf_listener = TransformListener()
# self.gripper_client = actionlib.SimpleActionClient("/robot/end_effector/left_gripper/gripper_action", GripperCommandAction)
# self.gripper_client.wait_for_server()
# rospy.loginfo("...connected.")
rospy.sleep(2.0)
def updateScene(self, remove_collision = False):
if remove_collision:
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()
return
# find objects
self.cubes = []
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(15.0))
find_result = self.find_client.get_result()
# rospy.loginfo(find_result)
# 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()
# insert objects to scene
idx = -1
print(find_result.objects)
# print(find_result.support_surfaces)
# for obj in find_result.objects:
# idx += 1
# print(idx)
# obj.object.name = "object%d" % idx
# self.scene.addSolidPrimitive(obj.object.name,
# obj.object.primitives[0],
# obj.object.primitive_poses[0],
# wait = False)
# self.cubes.append(obj.object.primitive_poses[0])
#
# for obj in find_result.support_surfaces:
# # extend surface to floor, and make wider since we have narrow field of view
# height = obj.primitive_poses[0].position.z
# obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0],
# 1.5, # wider
# obj.primitives[0].dimensions[2] + height]
# obj.primitive_poses[0].position.z += -height / 2.0
#
# # add to scene
# self.scene.addSolidPrimitive(obj.name,
# obj.primitives[0],
# obj.primitive_poses[0],
# wait = False)
#
# self.scene.waitForSync()
#
# # store for grasping
# self.objects = find_result.objects
# self.surfaces = find_result.support_surfaces
def getGraspableCube(self):
graspable = None
for obj in self.objects:
# need grasps
if len(obj.grasps) < 1:
continue
# check size
if obj.object.primitives[0].dimensions[0] < 0.05 or \
obj.object.primitives[0].dimensions[0] > 0.07 or \
obj.object.primitives[0].dimensions[0] < 0.05 or \
obj.object.primitives[0].dimensions[0] > 0.07 or \
obj.object.primitives[0].dimensions[0] < 0.05 or \
obj.object.primitives[0].dimensions[0] > 0.07:
continue
# has to be on table
if obj.object.primitive_poses[0].position.z < 0.5:
continue
return obj.object, obj.grasps
# nothing detected
return None, None
def get_sequence(self, seq_list):
start = [self.objects[seq_list[0]].object, self.objects[seq_list[0]].grasps] or [None, None]
target = [self.objects[seq_list[1]].object, self.objects[seq_list[1]].grasps] or [None, None]
res = {'start': start, 'target': target}
return res
def get_transform_pose(self, pose):
point = PoseStamped()
point.header.frame_id = 'base_link'
point.header.stamp = rospy.Time()
point.pose = pose
return self.tf_listener.transformPose('/map', point).pose.position
def getPlaceLocation(self):
pass
def pick(self, block, grasps):
success, pick_result = self.pickplace.pick_with_retry(block.name,
grasps,
retries = 5,
support_name = block.support_surface,
scene = self.scene)
self.pick_result = pick_result
return success
def place(self, block, pose_stamped):
places = list()
l = PlaceLocation()
l.place_pose.pose = pose_stamped.pose
l.place_pose.header.frame_id = pose_stamped.header.frame_id
# copy the posture, approach and retreat from the grasp used
l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach
l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
places.append(copy.deepcopy(l))
# create another several places, rotate each by 360/m degrees in yaw direction
m = 16 # number of possible place poses
pi = 3.141592653589
for i in range(0, m - 1):
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 2 * pi / m)
places.append(copy.deepcopy(l))
success, place_result = self.pickplace.place_with_retry(block.name,
places,
scene = self.scene,
retries = 5)
# print(success)
return success
def tuck(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
# pose =[ 1.58, -0.056, -0.01, -1.31, -0.178, -0.13, 0.16]
gripper_goal = GripperCommandGoal()
gripper_goal.command.max_effort = 0.0
gripper_goal.command.position = 0.09
self.gripper_client.send_goal(gripper_goal)
self.gripper_client.wait_for_result(rospy.Duration(5))
start = rospy.Time.now()
while rospy.Time.now() - start <= rospy.Duration(10.0): # rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
return
def gripper_opening(self, opening = 0.09):
gripper_goal = GripperCommandGoal()
gripper_goal.command.max_effort = 0.0
gripper_goal.command.position = opening
self.gripper_client.send_goal(gripper_goal)
self.gripper_client.wait_for_result(rospy.Duration(5))
class GraspingClient(object):
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.move_group = MoveGroupInterface("arm", "base_link")
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(
find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
def updateScene(self):
# find objects
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
find_result = self.find_client.get_result()
"""
might be useful if we want to pick and place multiple objects
"""
# 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()
# insert objects to scene
idx = -1
for obj in find_result.objects:
idx += 1
obj.object.name = "object%d" % idx
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
wait=False)
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [
obj.primitives[0].dimensions[0],
1.5, # wider
obj.primitives[0].dimensions[2] + height
]
obj.primitive_poses[0].position.z += -height / 2.0
# add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
wait=False)
self.scene.waitForSync()
# store for grasping
self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
def getGraspableCube(self):
graspable = None
for obj in self.objects:
# need grasps
if len(obj.grasps) < 1:
continue
# check size
if obj.object.primitives[0].dimensions[0] < 0.05 or \
obj.object.primitives[0].dimensions[0] > 0.07 or \
obj.object.primitives[0].dimensions[0] < 0.05 or \
obj.object.primitives[0].dimensions[0] > 0.07 or \
obj.object.primitives[0].dimensions[0] < 0.05 or \
obj.object.primitives[0].dimensions[0] > 0.07:
continue
# has to be on table
if obj.object.primitive_poses[0].position.z < 0.5:
continue
return obj.object, obj.grasps
# nothing detected
return None, None
def getSupportSurface(self, name):
for surface in self.support_surfaces:
if surface.name == name:
return surface
return None
def getPlaceLocation(self):
pass
def pick(self, block, grasps):
success, pick_result = self.pickplace.pick_with_retry(
block.name,
grasps,
support_name=block.support_surface,
scene=self.scene)
self.pick_result = pick_result
return success
def place(self, block, pose_stamped):
places = list()
l = PlaceLocation()
l.place_pose.pose = pose_stamped.pose
l.place_pose.header.frame_id = pose_stamped.header.frame_id
"""
can change the place location based on the pick location. Let the pick location be the origin.
"""
# copy the posture, approach and retreat from the grasp used
l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach
l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
places.append(copy.deepcopy(l))
# create another several places, rotate each by 360/m degrees in yaw direction
m = 16 # number of possible place poses
pi = 3.141592653589
for i in range(0, m - 1):
l.place_pose.pose = rotate_pose_msg_by_euler_angles(
l.place_pose.pose, 0, 0, 2 * pi / m)
places.append(copy.deepcopy(l))
success, place_result = self.pickplace.place_with_retry(
block.name, places, scene=self.scene)
return success
class Grasping(object):
def __init__(self):
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.scene = PlanningSceneInterface("base_link")
self.move_group = MoveGroupInterface("arm", "base_link")
find_objects = "basic_grasping_perception/find_objects"
self.find_client = actionlib.SimpleActionClient(
find_objects, FindGraspableObjectsAction)
self.find_client.wait_for_server()
def pickup(self, block, grasps):
rospy.sleep(1)
success, pick_result = self.pickplace.pick_with_retry(
block.name,
grasps,
support_name=block.support_surface,
scene=self.scene)
self.pick_result = pick_result
return success
#swaps two blocks positions.
def swapBlockPos(self, block1Pos, block2Pos):
#intermediate point for movement
self.armIntermediatePose()
#intermediate positon on the table, used for short term storage of the first block manipulated in the sorting.
posIntermediate = np.array([0.725, 0])
self.armIntermediatePose()
# Get block to pick
while not rospy.is_shutdown():
rospy.loginfo("Picking object...")
self.updateScene()
cube, grasps = self.getGraspableObject(block1Pos)
if cube == None:
rospy.logwarn("Perception failed.")
continue
# Pick the block
if self.pickup(cube, grasps):
break
rospy.logwarn("Grasping failed.")
#self.tuck()
#Place the block
while not rospy.is_shutdown():
rospy.loginfo("Placing object...")
pose = PoseStamped()
pose.pose = cube.primitive_poses[0]
pose.pose.position.z += 0.05
pose.header.frame_id = cube.header.frame_id
if self.place(cube, pose, posIntermediate):
break
rospy.logwarn("Placing failed.")
#place block 2 in block 1's
self.armIntermediatePose()
# Get block to pick
while not rospy.is_shutdown():
rospy.loginfo("Picking object...")
self.updateScene()
cube, grasps = self.getGraspableObject(block2Pos)
if cube == None:
rospy.logwarn("Perception failed.")
continue
# Pick the block
if self.pickup(cube, grasps):
break
rospy.logwarn("Grasping failed.")
while not rospy.is_shutdown():
rospy.loginfo("Placing object...")
pose = PoseStamped()
pose.pose = cube.primitive_poses[0]
pose.pose.position.z += 0.05
pose.header.frame_id = cube.header.frame_id
if self.place(cube, pose, block1Pos):
break
rospy.logwarn("Placing failed.")
#place block1 in block 2's spot
self.armIntermediatePose()
while not rospy.is_shutdown():
rospy.loginfo("Picking object...")
self.updateScene()
cube, grasps = self.getGraspableObject(posIntermediate)
if cube == None:
rospy.logwarn("Perception failed.")
continue
# Pick the block
if self.pickup(cube, grasps):
break
rospy.logwarn("Grasping failed.")
#self.tuck()
self.armIntermediatePose()
while not rospy.is_shutdown():
rospy.loginfo("Placing object...")
pose = PoseStamped()
pose.pose = cube.primitive_poses[0]
pose.pose.position.z += 0.05
pose.header.frame_id = cube.header.frame_id
if self.place(cube, pose, block2Pos):
break
rospy.logwarn("Placing failed.")
return
def place(self, block, pose_stamped, placePos):
rospy.sleep(1)
#creates a list of place positons for the block, with a specified x, y, and z.
places = list()
l = PlaceLocation()
l.place_pose.pose = pose_stamped.pose
l.place_pose.header.frame_id = "base_link"
l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach
l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
#this x and y value are input as placePos through the function call.
l.place_pose.pose.position.x = placePos[0]
l.place_pose.pose.position.y = placePos[1]
places.append(copy.deepcopy(l))
#success = self.pickplace.place_with_retry(block.name, places, scene = self.scene)
#return success
## create another several places, rotate each by 360/m degrees in yaw direction
m = 16 # number of possible place poses
pi = 3.141592653589
for i in range(0, m - 1):
l.place_pose.pose = rotate_pose_msg_by_euler_angles(
l.place_pose.pose, 0, 0, 2 * pi / m)
places.append(copy.deepcopy(l))
success, place_result = self.pickplace.place_with_retry(
block.name, places, scene=self.scene)
return success
def updateScene(self):
rospy.sleep(1)
# find objectsw
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
find_result = self.find_client.get_result()
# 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()
# insert objects to scene
idx = -1
for obj in find_result.objects:
idx += 1
obj.object.name = "object%d" % idx
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
use_service=False)
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [
obj.primitives[0].dimensions[0],
1.5, # wider
obj.primitives[0].dimensions[2] + height
]
obj.primitive_poses[0].position.z += -height / 2.0
# add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
use_service=True)
self.scene.waitForSync()
# store for grasping
self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
def getSupportSurface(self, name):
for surface in self.support_surfaces:
if surface.name == name:
return surface
return None
def getPlaceLocation(self):
pass
def getGraspableObject(self, pos):
graspable = None
for obj in self.objects:
if len(obj.grasps) < 1:
continue
if (obj.object.primitives[0].dimensions[0] < 0.05 or \
obj.object.primitives[0].dimensions[0] > 0.07 or \
obj.object.primitives[0].dimensions[1] < 0.05 or \
obj.object.primitives[0].dimensions[1] > 0.07 or \
obj.object.primitives[0].dimensions[2] < 0.05 or \
obj.object.primitives[0].dimensions[2] > 0.07):
continue
if (obj.object.primitive_poses[0].position.z < 0.3) or \
(obj.object.primitive_poses[0].position.y > pos[1]+0.05) or \
(obj.object.primitive_poses[0].position.y < pos[1]-0.05) or \
(obj.object.primitive_poses[0].position.x > pos[0]+0.05) or \
(obj.object.primitive_poses[0].position.x < pos[0]-0.05):
continue
return obj.object, obj.grasps
return None, None
def armToXYZ(self, x, y, z):
rospy.sleep(1)
#new pose_stamped of the end effector that moves the arm out of the way of the vision for planning.
intermediatePose = PoseStamped()
intermediatePose.header.frame_id = 'base_link'
#position
intermediatePose.pose.position.x = x
intermediatePose.pose.position.y = y
intermediatePose.pose.position.z = z
#quaternion for the end position
intermediatePose.pose.orientation.w = 1
while not rospy.is_shutdown():
result = self.move_group.moveToPose(intermediatePose,
"wrist_roll_link")
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def armIntermediatePose(self):
self.armToXYZ(0.1, -0.7, 0.9)
def tuck(self):
joints = [
"shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
class GraspingClient(object):
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.move_group = MoveGroupInterface("arm", "base_link")
# 初始化需要使用move group控制的机械臂中的arm group
self.arm = MoveGroupCommander('arm')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
self.arm.set_goal_position_tolerance(0.01)
self.arm.set_goal_orientation_tolerance(0.05)
# 当运动规划失败后，允许重新规划
self.arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
self.arm.set_pose_reference_frame(reference_frame)
# 设置每次运动规划的时间限制：5s
self.arm.set_planning_time(5)
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
def updateScene(self):
# find objects
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
find_result = self.find_client.get_result()
# 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()
# insert objects to scene
objects = list()
idx = -1
for obj in find_result.objects:
idx += 1
obj.object.name = "object%d"%idx
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
wait = False)
if obj.object.primitive_poses[0].position.x < 0.85:
objects.append([obj, obj.object.primitive_poses[0].position.z])
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0],
1.5, # wider
obj.primitives[0].dimensions[2] + height]
obj.primitive_poses[0].position.z += -height/2.0
# add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
wait = False)
self.scene.waitForSync()
# store for grasping
#self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
# store graspable objects by Z
objects.sort(key=lambda object: object[1])
objects.reverse()
self.objects = [object[0] for object in objects]
#for object in objects:
# print(object[0].object.name, object[1])
#exit(-1)
def getGraspableObject(self):
graspable = None
for obj in self.objects:
# need grasps
if len(obj.grasps) < 1:
continue
# check size
if obj.object.primitives[0].dimensions[0] < 0.03 or \
obj.object.primitives[0].dimensions[0] > 0.25 or \
obj.object.primitives[0].dimensions[0] < 0.03 or \
obj.object.primitives[0].dimensions[0] > 0.25 or \
obj.object.primitives[0].dimensions[0] < 0.03 or \
obj.object.primitives[0].dimensions[0] > 0.25:
continue
# has to be on table
if obj.object.primitive_poses[0].position.z < 0.5:
continue
print obj.object.primitive_poses[0], obj.object.primitives[0]
return obj.object, obj.grasps
# nothing detected
return None, None
def getSupportSurface(self, name):
for surface in self.support_surfaces:
if surface.name == name:
return surface
return None
def getPlaceLocation(self):
pass
def pick(self, block, grasps):
success, pick_result = self.pickplace.pick_with_retry(block.name,
grasps,
support_name=block.support_surface,
scene=self.scene)
self.pick_result = pick_result
return success
def place(self, block, pose_stamped):
places = list()
l = PlaceLocation()
l.place_pose.pose = pose_stamped.pose
l.place_pose.header.frame_id = pose_stamped.header.frame_id
# copy the posture, approach and retreat from the grasp used
l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach
l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
places.append(copy.deepcopy(l))
# create another several places, rotate each by 360/m degrees in yaw direction
m = 16 # number of possible place poses
pi = 3.141592653589
for i in range(0, m-1):
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 2 * pi / m)
places.append(copy.deepcopy(l))
success, place_result = self.pickplace.place_with_retry(block.name,
places,
scene=self.scene)
return success
def check_collision(self, pose): # [0, 0, 0, 0, 0, 0, 0]
self.arm.set_joint_value_target(pose)
# 控制机械臂完成运动
#print self.arm.go()
#self.arm.set_joint_value_target([1.5, -0.6, 3.0, 1.0, 3.0, 1.0, 3.0])
return self.arm.go()
def tuck(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def stow(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def intermediate_stow(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [0.7, -0.3, 0.0, -0.3, 0.0, -0.57, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def dyn_wedge():
pub = rospy.Publisher("finished", String, queue_size=10)
# Initialize MoveIt scene
p = PlanningSceneInterface("base")
arms_group = MoveGroupInterface("both_arms", "base")
rightarm_group = MoveGroupInterface("right_arm", "base")
leftarm_group = MoveGroupInterface("left_arm", "base")
# Create right arm instance
right_arm = baxter_interface.limb.Limb('right')
# Create right gripper instance
right_gripper = baxter_interface.Gripper('right')
right_gripper.calibrate()
right_gripper.open()
right_gripper.close()
offset_zero_point=0.903
table_size_x = 0.714655654394
table_size_y = 1.05043717328
table_size_z = 0.729766045265
center_x = 0.457327827197
center_y = 0.145765166941
center_z = -0.538116977368
table_distance_from_gripper = -offset_zero_point+table_size_z+0.0275/2
j=0
k=0
# Initialize object list
objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11']
p.clear()
p.attachBox('table', table_size_x, table_size_y, table_size_z, center_x, center_y, center_z, 'base', touch_links=['pedestal'])
p.waitForSync()
# Move both arms to start state.
# Initial pose
rpos = PoseStamped()
rpos.header.frame_id = "base"
rpos.header.stamp = rospy.Time.now()
rpos.pose.position.x = 0.555
rpos.pose.position.y = 0.0
rpos.pose.position.z = 0.206 #0.18
rpos.pose.orientation.x = 1.0
rpos.pose.orientation.y = 0.0
rpos.pose.orientation.z = 0.0
rpos.pose.orientation.w = 0.0
lpos = PoseStamped()
lpos.header.frame_id = "base"
lpos.header.stamp = rospy.Time.now()
lpos.pose.position.x = 0.555
lpos.pose.position.y = 0.65
lpos.pose.position.z = 0.206#0.35
lpos.pose.orientation.x = 1.0
lpos.pose.orientation.y = 0.0
lpos.pose.orientation.z = 0.0
lpos.pose.orientation.w = 0.0
rightarm_group.moveToPose(rpos, "right_gripper", max_velocity_scaling_factor=1, plan_only=False)
leftarm_group.moveToPose(lpos, "left_gripper", max_velocity_scaling_factor=1, plan_only=False)
# Get the middle point between the two centroids
locs = rospy.wait_for_message("clasificacion", PoseArray)
if(len(locs.poses)!=0):
punto_medio1 = PoseStamped()
punto_medio1.header.frame_id = "base"
punto_medio1.header.stamp = rospy.Time.now()
punto_medio1.pose.position.x = locs.poses[0].position.x
punto_medio1.pose.position.y = locs.poses[0].position.y
punto_medio1.pose.position.z = -0.08
punto_medio1.pose.orientation.x = locs.poses[0].orientation.x
punto_medio1.pose.orientation.y = locs.poses[0].orientation.y
punto_medio1.pose.orientation.z = locs.poses[0].orientation.z
punto_medio1.pose.orientation.w = locs.poses[0].orientation.w
else:
sys.exit("No se encuentran los puntos")
# Get the middle point again after a few seconds
tiempo = 3
time.sleep(tiempo)
locs = rospy.wait_for_message("clasificacion", PoseArray)
if(len(locs.poses)!=0):
punto_medio2 = PoseStamped()
punto_medio2.header.frame_id = "base"
punto_medio2.header.stamp = rospy.Time.now()
punto_medio2.pose.position.x = locs.poses[0].position.x
punto_medio2.pose.position.y = locs.poses[0].position.y
punto_medio2.pose.position.z = -0.08
punto_medio2.pose.orientation.x = locs.poses[0].orientation.x
punto_medio2.pose.orientation.y = locs.poses[0].orientation.y
punto_medio2.pose.orientation.z = locs.poses[0].orientation.z
punto_medio2.pose.orientation.w = locs.poses[0].orientation.w
else:
sys.exit("No se encuentran los puntos")
# Calculate speed of objects
vel = (punto_medio2.pose.position.y - punto_medio1.pose.position.y) / tiempo
# Predict position within a certain time
nuevo_tiempo = 2.1
posicion = nuevo_tiempo * vel * 2
if(posicion>=0):
nueva_y = punto_medio2.pose.position.y - posicion
else:
nueva_y = punto_medio2.pose.position.y + posicion
orientacion = (-1) * 1/punto_medio2.pose.orientation.x
new_or = 0.26
if orientacion > 0:
new_or = -0.26
# If there is time enough to sweep the objects
if vel > -0.08:
start = time.time()
punto_medio2.pose.position.y = nueva_y - nueva_y/2
punto_medio2.pose.position.x = punto_medio1.pose.position.x - 0.03
punto_medio2.pose = rotate_pose_msg_by_euler_angles(punto_medio2.pose, 0.0, 0.0, orientacion - new_or)
rightarm_group.moveToPose(punto_medio2, "right_gripper", max_velocity_scaling_factor=1, plan_only=False)
end = time.time()
tiempo = end - start
while(nuevo_tiempo - tiempo >= 1):
end = time.time()
nuevo_tiempo = end - start
else:
punto_medio2.pose.position.y = nueva_y
print(punto_medio2.pose.position.y)
punto_medio2.pose.position.x = punto_medio1.pose.position.x
punto_medio2.pose.position.y += 0.05
punto_medio2.pose = rotate_pose_msg_by_euler_angles(punto_medio2.pose, 0.0, 0.0, orientacion)
rightarm_group.moveToPose(punto_medio2, "right_gripper", max_velocity_scaling_factor=1, plan_only=False)
obj = punto_medio2.pose.position.y + 0.16
neg = 1
while punto_medio2.pose.position.y < obj:
punto_medio2.pose.position.y += 0.03
# Shake arm
punto_medio2.pose.position.x += neg * 0.09
rightarm_group.moveToPose(punto_medio2, "right_gripper", max_velocity_scaling_factor=1, plan_only=False)
neg = (-1)*neg
time.sleep(2)
rightarm_group.moveToPose(rpos, "right_gripper", max_velocity_scaling_factor=1, plan_only=False)
freemem = "free"
pub.publish(freemem)
class GripperInterfaceClient:
def __init__(self):
self.joint_names = ["l_gripper_finger_joint", "r_gripper_finger_joint"]
self.client = actionlib.SimpleActionClient("gripper_controller/gripper_action", GripperCommandAction)
rospy.loginfo("Waiting for gripper_controller...")
self.client.wait_for_server()
rospy.loginfo("...connected.")
self.scene = PlanningSceneInterface("base_link")
self.attached_object_publisher = rospy.Publisher(
"attached_collision_object", AttachedCollisionObject, queue_size=10
)
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True, plan_only=True)
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
self.debug_index = -1
self.graspables = []
def execute_trajectory(self, trajectory):
goal_position = trajectory.points[-1].positions
print "==========", goal_position
command_goal = GripperCommandGoal()
command_goal.command.position = goal_position[0] + goal_position[1]
command_goal.command.max_effort = 50.0 # Placeholder. TODO Figure out a better way to compute this
self.client.send_goal(command_goal)
self.client.wait_for_result()
def move_to(self, goal_position, max_effort):
command_goal = GripperCommandGoal()
command_goal.command.position = goal_position
command_goal.command.max_effort = max_effort
self.client.send_goal(command_goal)
self.client.wait_for_result()
def updateScene(self):
# find objects
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
find_result = self.find_client.get_result()
# remove previous objects
for name in self.scene.getKnownCollisionObjects():
self.scene.removeCollisionObject(name, wait=False)
for name in self.scene.getKnownAttachedObjects():
self.scene.removeAttachedObject(name, wait=False)
rospy.sleep(0.5) # Gets rid of annoying error messages stemming from race condition.
self.scene.waitForSync()
# insert objects to scene
idx = -1
for obj in find_result.objects:
idx += 1
obj.object.name = "object%d" % idx
self.scene.addSolidPrimitive(
obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], wait=False
)
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [
obj.primitives[0].dimensions[0] + 0.1,
1.5, # wider
obj.primitives[0].dimensions[2] + height,
]
obj.primitive_poses[0].position.z += -height / 2.0
# add to scene
self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], wait=False)
self.scene.waitForSync()
# store for grasping
self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
def getGraspableCube(self):
graspable = None
for obj in self.objects:
# need grasps
if len(obj.grasps) < 1:
continue
# check size
# if obj.object.primitives[0].dimensions[0] < 0.05 or \
# obj.object.primitives[0].dimensions[0] > 0.07 or \
# obj.object.primitives[0].dimensions[0] < 0.05 or \
# obj.object.primitives[0].dimensions[0] > 0.07 or \
# obj.object.primitives[0].dimensions[0] < 0.05 or \
# obj.object.primitives[0].dimensions[0] > 0.07:
# continue
# has to be on table
# if obj.object.primitive_poses[0].position.z < 0.5:
# continue
return obj.object, obj.grasps
# nothing detected
return None, None
def set_target_object(self, primitive_type, target_pose, dimensions):
primitive_types = {"BOX": 1, "SPHERE": 2, "CYLINDER": 3, "CONE": 4}
self.sought_object = Object()
primitive = SolidPrimitive()
primitive.type = primitive_types[primitive_type.upper()]
primitive.dimensions = dimensions
self.sought_object.primitives.append(primitive)
p = Pose()
p.position.x = target_pose[0]
p.position.y = target_pose[1]
p.position.z = target_pose[2]
quaternion = quaternion_from_euler(target_pose[3], target_pose[4], target_pose[5])
p.orientation.x = quaternion[0]
p.orientation.y = quaternion[1]
p.orientation.z = quaternion[2]
p.orientation.w = quaternion[3]
self.sought_object.primitive_poses.append(p)
def find_graspable_object(self, tolerance=0.01):
self.updateScene()
self.current_grasping_target = None
for obj in self.objects:
# Must have grasps
if len(obj.grasps) < 1:
continue
# Compare to sought object
detected_object_dimensions = np.array(obj.object.primitives[0].dimensions)
sought_object_dimensions = np.array(self.sought_object.primitives[0].dimensions)
try:
difference = detected_object_dimensions - sought_object_dimensions
print "===DEBUG: Difference: ", difference
mag_diff = np.linalg.norm(difference)
print "===DEBUG: mag_diff: ", mag_diff
if mag_diff <= tolerance:
self.current_grasping_target = obj
tolerance = mag_diff
print "===DEBUG: Tolerance: ", tolerance
else:
rospy.loginfo("Object dimensions do not match. Trying another object...")
except:
rospy.loginfo("Object geometries do not match. Trying another object...")
if self.current_grasping_target is None:
tolerance += 0.01
self.find_graspable_object(tolerance=tolerance)
# Nothing detected
def computeGraspToPickMatrix(self):
pick_rot = self.current_grasping_target.object.primitive_poses[0].orientation
grasp_rot = self.pick_result.grasp.grasp_pose.pose.orientation
pick_translation_distance = self.pick_result.grasp.pre_grasp_approach.desired_distance
pick_quaternion = [pick_rot.x, pick_rot.y, pick_rot.z, pick_rot.w]
grasp_quaternion = [grasp_rot.x, grasp_rot.y, grasp_rot.z, grasp_rot.w]
pick_to_grasp_quaternion = quaternion_multiply(quaternion_inverse(grasp_quaternion), pick_quaternion)
rotation_matrix = quaternion_matrix(pick_to_grasp_quaternion)
translation = [pick_translation_distance, 0, 0]
rotation_matrix[[0, 1, 2], 3] = translation
pick_to_grasp_matrix = np.mat(rotation_matrix)
grasp_to_pick_matrix = pick_to_grasp_matrix.getI()
return grasp_to_pick_matrix
def computePlaceToBaseMatrix(self, place):
place_quaternion = place[3:]
rotation_matrix = quaternion_matrix(place_quaternion)
translation = place[0:3]
rotation_matrix[[0, 1, 2], 3] = translation
place_to_base_matrix = rotation_matrix
return place_to_base_matrix
def broadcastCurrentGraspingTargetTransform(self):
pose = self.current_grasping_target.object.primitive_poses[0]
br = tf2_ros.TransformBroadcaster()
t = geometry_msgs.msg.TransformStamped()
t.header.stamp = rospy.Time.now()
t.header.frame_id = "base_link"
t.child_frame_id = "current_grasping_target"
t.transform.translation.x = pose.position.x
t.transform.translation.y = pose.position.y
t.transform.translation.z = pose.position.z
t.transform.rotation.x = pose.orientation.x
t.transform.rotation.y = pose.orientation.y
t.transform.rotation.z = pose.orientation.z
t.transform.rotation.w = pose.orientation.w
br.sendTransform(t)
def plan_pick(self):
success, pick_result = self.pickplace.pick_with_retry(
self.current_grasping_target.object.name,
self.current_grasping_target.grasps,
support_name=self.current_grasping_target.object.support_surface,
scene=self.scene,
allow_gripper_support_collision=False,
planner_id="PRMkConfigDefault",
)
self.pick_result = pick_result
print "DEBUG: plan_pick(): ", self.scene.getKnownAttachedObjects()
if success:
rospy.loginfo("Planning succeeded.")
trajectory_approved = get_user_approval()
if trajectory_approved:
return pick_result.trajectory_stages
else:
rospy.loginfo("Plan rejected. Getting new grasps for replan...")
else:
rospy.logwarn("Planning failed. Getting new grasps for replan...")
self.find_graspable_object()
return self.plan_pick()
def plan_place(self, desired_pose):
places = list()
ps = PoseStamped()
# ps.pose = self.current_grasping_target.object.primitive_poses[0]
ps.header.frame_id = self.current_grasping_target.object.header.frame_id
grasp_to_pick_matrix = self.computeGraspToPickMatrix()
place_to_base_matrix = self.computePlaceToBaseMatrix(desired_pose)
grasp2_to_place_matrix = place_to_base_matrix * grasp_to_pick_matrix
position_vector = grasp2_to_place_matrix[0:-1, 3]
quaternion = quaternion_from_matrix(grasp2_to_place_matrix)
position_array = position_vector.getA1()
l = PlaceLocation()
direction_components = self.pick_result.grasp.pre_grasp_approach.direction.vector
l.place_pose.header.frame_id = ps.header.frame_id
l.place_pose.pose.position.x = position_array[0]
l.place_pose.pose.position.y = position_array[1]
l.place_pose.pose.position.z = position_array[2]
l.place_pose.pose.orientation.x = quaternion[0]
l.place_pose.pose.orientation.y = quaternion[1]
l.place_pose.pose.orientation.z = quaternion[2]
l.place_pose.pose.orientation.w = quaternion[3]
# copy the posture, approach and retreat from the grasp used
approach = copy.deepcopy(self.pick_result.grasp.pre_grasp_approach)
approach.desired_distance /= 2.0
l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
l.pre_place_approach = approach
l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
places.append(copy.deepcopy(l))
# create another several places, rotate each by 90 degrees in roll direction
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0)
places.append(copy.deepcopy(l))
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0)
places.append(copy.deepcopy(l))
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0)
places.append(copy.deepcopy(l))
print "DEBUG: Places: ", places[0]
success, place_result = self.pickplace.place_with_retry(
self.current_grasping_target.object.name,
places,
scene=self.scene,
allow_gripper_support_collision=False,
planner_id="PRMkConfigDefault",
)
if success:
rospy.loginfo("Planning succeeded.")
trajectory_approved = get_user_approval()
if trajectory_approved:
return place_result.trajectory_stages
else:
rospy.loginfo("Plan rejected. Replanning...")
else:
rospy.logwarn("Planning failed. Replanning...")
desired_pose[2] += 0.05
return self.plan_place(desired_pose)
def recognizeAttachedObject(self):
print "========= Recognizing attached object"
name = self.current_grasping_target.object.name
size_x = self.current_grasping_target.object.primitives[0].dimensions[0]
size_y = self.current_grasping_target.object.primitives[0].dimensions[1]
size_z = self.current_grasping_target.object.primitives[0].dimensions[2]
(x, y, z) = (0.03, 0.0, 0.0)
link_name = "gripper_link"
touch_links = ["l_gripper_finger_link", "r_gripper_finger_link", "gripper_link"]
detach_posture = None
weight = 0.0
wait = True
"""
object_x = self.current_grasping_target.object.primitive_poses[0].position.x
object_y = self.current_grasping_target.object.primitive_poses[0].position.y
object_z = self.current_grasping_target.object.primitive_poses[0].position.z
pick_rot = self.current_grasping_target.object.primitive_poses[0].orientation
grasp_rot = self.pick_result.grasp.grasp_pose.pose.orientation
grasp_position = self.pick_result.grasp.grasp_pose.pose.position
pick_translation_distance = self.pick_result.grasp.post_grasp_retreat.desired_distance
pick_quaternion = [pick_rot.x, pick_rot.y, pick_rot.z, pick_rot.w]
grasp_quaternion = [grasp_rot.x, grasp_rot.y, grasp_rot.z, grasp_rot.w]
pick_to_grasp_quaternion = quaternion_multiply(quaternion_inverse(grasp_quaternion), pick_quaternion)
grasp_to_base_matrix_array = quaternion_matrix(grasp_quaternion)
grasp_to_base_matrix = np.matrix(grasp_to_base_matrix_array)
displacement = grasp_to_base_matrix.dot(np.matrix([[pick_translation_distance-.03], [0], [0], [0]]))
print displacement
attached_location = list()
attached_location.append(object_x - displacement[0,0])
attached_location.append(object_y - displacement[1,0])
attached_location.append(object_z - displacement[2,0])
print attached_location
quaternion = Quaternion()
quaternion.x = pick_to_grasp_quaternion[0]
quaternion.y = pick_to_grasp_quaternion[1]
quaternion.z = pick_to_grasp_quaternion[2]
quaternion.w = pick_to_grasp_quaternion[3]
pose = Pose()
pose.position.x = attached_location[0]
pose.position.y = attached_location[1]
pose.position.z = attached_location[2]
pose.orientation = self.current_grasping_target.object.primitive_poses[0].orientation
collision_object = self.scene.makeSolidPrimitive(self.current_grasping_target.object.name,
self.current_grasping_target.object.primitives[0],
pose)
attached_collision_object = self.scene.makeAttached("gripper_link",
collision_object,
touch_links, detach_posture, 0.0)
self.attached_object_publisher.publish(attached_collision_object)
"""
self.scene.attachBox(
name,
size_x,
size_y,
size_z,
x,
y,
z,
link_name,
touch_links=touch_links,
detach_posture=detach_posture,
weight=weight,
wait=wait,
)
def removeCollisionObjects(self):
collision_object_names = self.scene.getKnownCollisionObjects()
print self.current_grasping_target.object.name
print collision_object_names
raw_input("press ENTER")
x = self.scene.getKnownAttachedObjects()
print x
raw_input("press Enter")
for name in collision_object_names:
if name != self.current_grasping_target.object.name:
self.scene.removeCollisionObject(name, wait=False)
def picknplace():
# Define initial parameters
p = PlanningSceneInterface("base")
g = MoveGroupInterface("both_arms", "base")
gr = MoveGroupInterface("right_arm", "base")
gl = MoveGroupInterface("left_arm", "base")
leftgripper = baxter_interface.Gripper('left')
leftgripper.calibrate()
leftgripper.open()
jts_both = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2', 'right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
jts_right = ['right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
jts_left = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2']
pos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519,1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719]
pos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384, 1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323]
lpos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519]
lpos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384]
rpos1 = [1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719]
rpos2 = [1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323]
# Clear planning scene
p.clear()
# Add table as attached object
p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal'])
# Move both arms to start state
g.moveToJointPosition(jts_both, pos1, plan_only=False)
# Get obj locations
temp = rospy.wait_for_message("Dpos", PoseArray)
locs = temp.poses
locs_x = []
locs_y = []
orien = []
size = []
for i in range(len(locs)):
locs_x.append(0.57 + locs[i].position.x)
locs_y.append(-0.011 + locs[i].position.y)
orien.append(locs[i].position.z*pi/180)
size.append(locs[i].orientation.x)
# Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene
ind_rmv = []
for i in range(0,len(locs)):
if (locs_y[i] > 0.42):
ind_rmv.append(i)
continue
for j in range(i,len(locs)):
if not (i == j):
if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018:
ind_rmv.append(i)
locs_x = del_meth(locs_x, ind_rmv)
locs_y = del_meth(locs_y, ind_rmv)
orien = del_meth(orien, ind_rmv)
size = del_meth(size, ind_rmv)
# Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes
if locs_x:
ig0 = itemgetter(0)
sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0))
locs_x = list(sorted_lists[1])
locs_y = list(sorted_lists[2])
orien = list(sorted_lists[3])
size = list(sorted_lists[0])
# Loop to continue pick and place until all objects are cleared from table
j=0
k=0
while locs_x:
p.clear() # CLear planning scene of all collision objects
# Attach table as attached collision object to base of baxter
p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal'])
xn = locs_x[0]
yn = locs_y[0]
zn = -0.06
thn = orien[0]
sz = size[0]
if thn > pi/4:
thn = -1*(thn%(pi/4))
# Add collision objects into planning scene
objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11']
for i in range(1,len(locs_x)):
p.addCube(objlist[i], 0.05, locs_x[i], locs_y[i], -0.05)
p.waitForSync()
# Move both arms to pos2 (Right arm away and left arm on table)
g.moveToJointPosition(jts_both, pos2, plan_only=False)
# Move left arm to pick object and pick object
pickgoal = PoseStamped()
pickgoal.header.frame_id = "base"
pickgoal.header.stamp = rospy.Time.now()
pickgoal.pose.position.x = xn
pickgoal.pose.position.y = yn
pickgoal.pose.position.z = zn+0.1
pickgoal.pose.orientation.x = 1.0
pickgoal.pose.orientation.y = 0.0
pickgoal.pose.orientation.z = 0.0
pickgoal.pose.orientation.w = 0.0
gl.moveToPose(pickgoal, "left_gripper", plan_only=False)
# Orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles
pickgoal.pose = rotate_pose_msg_by_euler_angles(pickgoal.pose, 0.0, 0.0, thn)
gl.moveToPose(pickgoal, "left_gripper", plan_only=False)
pickgoal.pose.position.z = zn
gl.moveToPose(pickgoal, "left_gripper", max_velocity_scaling_factor = 0.14, plan_only=False)
leftgripper.close()
pickgoal.pose.position.z = zn+0.1
gl.moveToPose(pickgoal, "left_gripper", plan_only=False)
# Move both arms to pos1
g.moveToJointPosition(jts_both, pos1, plan_only=False)
# Get obj locations
temp = rospy.wait_for_message("Dpos", PoseArray)
locs = temp.poses
locs_x = []
locs_y = []
orien = []
size = []
for i in range(len(locs)):
locs_x.append(0.57 + locs[i].position.x)
locs_y.append(-0.011 + locs[i].position.y)
orien.append(locs[i].position.z*pi/180)
size.append(locs[i].orientation.x)
# Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene
ind_rmv = []
for i in range(0,len(locs)):
if (locs_y[i] > 0.42):
ind_rmv.append(i)
continue
for j in range(i,len(locs)):
if not (i == j):
if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018:
ind_rmv.append(i)
locs_x = del_meth(locs_x, ind_rmv)
locs_y = del_meth(locs_y, ind_rmv)
orien = del_meth(orien, ind_rmv)
size = del_meth(size, ind_rmv)
# Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes
if locs_x:
ig0 = itemgetter(0)
sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0))
locs_x = list(sorted_lists[1])
locs_y = list(sorted_lists[2])
orien = list(sorted_lists[3])
size = list(sorted_lists[0])
# Place object
stleft = PoseStamped()
stleft.header.frame_id = "base"
stleft.header.stamp = rospy.Time.now()
stleft.pose.position.x = 0.65
stleft.pose.position.y = 0.55
stleft.pose.position.z = 0.1
stleft.pose.orientation.x = 1.0
stleft.pose.orientation.y = 0.0
stleft.pose.orientation.z = 0.0
stleft.pose.orientation.w = 0.0
# Stack objects (large cubes) if size if greater than some threshold
if sz > 16.:
stleft.pose.position.x = 0.65
stleft.pose.position.y = 0.7
stleft.pose.position.z = -0.04+(k*0.05)
k += 1
gl.moveToPose(stleft, "left_gripper", plan_only=False)
leftgripper.open()
stleft.pose.position.z = 0.3
gl.moveToPose(stleft, "left_gripper", plan_only=False)
class MoveItDemo:
def __init__(self):
rospy.init_node('moveit_demo')
# We need a tf2 listener to convert poses into arm reference base
try:
self._tf2_buff = tf2_ros.Buffer()
self._tf2_list = tf2_ros.TransformListener(self._tf2_buff)
except rospy.ROSException as err:
rospy.logerr("MoveItDemo: could not start tf buffer client: " + str(err))
raise err
self.gripper_opened = [rospy.get_param(GRIPPER_PARAM + "/max_opening") - 0.01]
self.gripper_closed = [rospy.get_param(GRIPPER_PARAM + "/min_opening") + 0.01]
self.gripper_neutral = [rospy.get_param(GRIPPER_PARAM + "/neutral",
(self.gripper_opened[0] + self.gripper_closed[0])/2.0) ]
self.gripper_tighten = rospy.get_param(GRIPPER_PARAM + "/tighten", 0.0)
# Use the planning scene object to add or remove objects
self.scene = PlanningSceneInterface(REFERENCE_FRAME)
# 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('target_pose', PoseStamped, queue_size=10)
# Create a dictionary to hold object colors
self.colors = dict()
self.server = InteractiveMarkerServer("obj_find")
# Initialize the move group for the right arm
#######arm = MoveGroupCommander(GROUP_NAME_ARM)
self.arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
#######gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
self.gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
#######end_effector_link = arm.get_end_effector_link()
self.end_effector_link = self.arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
#######arm.set_goal_position_tolerance(0.05)
#######arm.set_goal_orientation_tolerance(0.1)
self.arm.set_goal_position_tolerance(0.05)
self.arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
#######arm.allow_replanning(True)
self.arm.allow_replanning(True)
# Set the right arm reference frame
#######arm.set_pose_reference_frame(REFERENCE_FRAME)
self.arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
#######arm.set_planning_time(15)
self.arm.set_planning_time(15)
# Set a limit on the number of pick attempts before bailing
#######max_pick_attempts = 1
self.max_pick_attempts = 1
# Set a limit on the number of place attempts
########max_place_attempts = 3
self.max_place_attempts = 3
# Give the scene a chance to catch up
rospy.sleep(2)
'''
rospy.loginfo("Connecting to basic_grasping_perception/find_objects...")
find_objects = actionlib.SimpleActionClient("basic_grasping_perception/find_objects", FindGraspableObjectsAction)
find_objects.wait_for_server()
rospy.loginfo("...connected")
rospy.sleep(1)
'''
self.arm.set_named_target('right_up')
if self.arm.go() != True:
rospy.logwarn(" Go failed")
self.gripper.set_joint_value_target(self.gripper_opened)
if self.gripper.go() != True:
rospy.logwarn(" Go failed")
rospy.sleep(1)
# Initialize the grasping goal
#goal = FindGraspableObjectsGoal()
#goal.plan_grasps = False
'''
find_objects.send_goal(goal)
find_objects.wait_for_result(rospy.Duration(5.0))
find_result = find_objects.get_result()
rospy.loginfo("Found %d objects" %len(find_result.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.scene._colors = dict()
# Use the nearest object on the table as the target
self.target_pose = PoseStamped()
self.target_pose.header.frame_id = REFERENCE_FRAME
self.target_id = 'target'
self.target_size = None
self.the_object = None
self.the_object_dist = 0.30
self.the_object_dist_min = 0.10
self.count = -1
#for obj in find_result.objects:
# count +=1
#dx = obj.object.primitive_poses[0].position.x
#dy = obj.object.primitive_poses[0].position.y
dx = 0.25
dy = 0.0
d = math.sqrt((dx * dx) + (dy * dy))
m = Marker()
m.type = Marker.CUBE
m.scale.x = 0.02
m.scale.y = 0.02
m.scale.z = 0.05
m.color.r = 0.0
m.color.g = 0.5
m.color.b = 0.5
m.color.a = 1.0
int_marker = InteractiveMarker()
int_marker.header.frame_id = "base_link"
int_marker.name = "block_1"
int_marker.pose.position.x = dx
int_marker.pose.position.y = dy
int_marker.pose.position.z = 0.04
int_marker.pose.orientation.x = 0.0
int_marker.pose.orientation.y = 0.0
int_marker.pose.orientation.z = 0.0
int_marker.pose.orientation.w = 1.0
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
int_marker.controls.append(control)
control.markers.append(m)
control.always_visible = True
int_marker.controls.append(control)
self.server.insert(int_marker,self.processFeedback)
self.server.applyChanges()
def processFeedback(self,feedback):
#p.pose.position = feedback.pose.position
#self.dist_xy.pose.position = feedback.pose.position
old_pose = Pose()
new_pose = Pose()
result = None
if feedback.event_type == visualization_msgs.msg.InteractiveMarkerFeedback.MOUSE_DOWN:
old_pose.position.x = feedback.pose.position.x
old_pose.position.y = feedback.pose.position.y
result = self.pick_pose(old_pose)
print "old pose is " + str(old_pose.position.x) + " , " + str(old_pose.position.y)
if feedback.event_type == visualization_msgs.msg.InteractiveMarkerFeedback.MOUSE_UP:
new_pose.position.x = feedback.pose.position.x
new_pose.position.y = feedback.pose.position.y
self.pick_and_place_pose(old_pose, new_pose)
print "new pose is " + str(new_pose.position.x) + " , " + str(new_pose.position.y)
def pick_and_place_pose(self,pick_pose,place_pose):
pass
def pick_pose(self, pick_pose):
moveit_commander.roscpp_initialize(sys.argv)
dx = pick_pose.position.x
dy = pick_pose.position.y
d = math.sqrt((dx * dx) + (dy * dy))
if d < self.the_object_dist and d > self.the_object_dist_min:
#if dx > the_object_dist_xmin and dx < the_object_dist_xmax:
# if dy > the_object_dist_ymin and dy < the_object_dist_ymax:
rospy.loginfo("object is in the working zone")
self.the_object_dist = d
self.the_object = self.count
self.target_size = [0.02, 0.02, 0.05]
#target_pose.pose.position.x = obj.object.primitive_poses[0].position.x + target_size[0] / 2.0
#target_pose.pose.position.y = obj.object.primitive_poses[0].position.y
self.target_pose.pose.position.x = dx + self.target_size[0] / 2.0
self.target_pose.pose.position.y = dy
self.target_pose.pose.position.z = 0.04
self.target_pose.pose.orientation.x = 0.0
self.target_pose.pose.orientation.y = 0.0
self.target_pose.pose.orientation.z = 0.0
self.target_pose.pose.orientation.w = 1.0
# wait for the scene to sync
self.scene.waitForSync()
self.scene.setColor(self.target_id, 223.0/256.0, 90.0/256.0, 12.0/256.0)
self.scene.sendColors()
grasp_pose = self.target_pose
grasp_pose.pose.position.y -= self.target_size[1] / 2.0
grasp_pose.pose.position.x += self.target_size[0]
grasps = self.make_grasps(grasp_pose, [self.target_id], [self.target_size[1] - self.gripper_tighten])
# Track success/failure and number of attempts for pick operation
'''
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
'''
# Track success/failure and number of attempts for pick operation
result = None
n_attempts = 0
# Set the start state to the current state
self.arm.set_start_state_to_current_state()
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < self.max_pick_attempts:
result = self.arm.pick(self.target_id, grasps)
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
rospy.sleep(1.0)
result = self.arm.pick(self.target_id, grasps)
else:
rospy.loginfo("object is not in the working zone")
rospy.sleep(1)
self.arm.set_named_target('right_up')
self.arm.go()
# Open the gripper to the neutral position
self.gripper.set_joint_value_target(self.gripper_neutral)
self.gripper.go()
rospy.sleep(1)
return result
# Get the gripper posture as a JointTrajectory
def make_gripper_posture(self, joint_positions):
# Initialize the joint trajectory for the gripper joints
t = JointTrajectory()
# Set the joint names to the gripper joint names
t.header.stamp = rospy.get_rostime()
t.joint_names = GRIPPER_JOINT_NAMES
# Initialize a joint trajectory point to represent the goal
tp = JointTrajectoryPoint()
# Assign the trajectory joint positions to the input positions
tp.positions = joint_positions
# Set the gripper effort
tp.effort = GRIPPER_EFFORT
tp.time_from_start = rospy.Duration(0.0)
# Append the goal point to the trajectory points
t.points.append(tp)
# Return the joint trajectory
return t
# Generate a gripper translation in the direction given by vector
def make_gripper_translation(self, min_dist, desired, vector):
# Initialize the gripper translation object
g = GripperTranslation()
# Set the direction vector components to the input
g.direction.vector.x = vector[0]
g.direction.vector.y = vector[1]
g.direction.vector.z = vector[2]
# The vector is relative to the gripper frame
g.direction.header.frame_id = GRIPPER_FRAME
# Assign the min and desired distances from the input
g.min_distance = min_dist
g.desired_distance = desired
return g
# Generate a list of possible grasps
def make_grasps(self, initial_pose_stamped, allowed_touch_objects, grasp_opening=[0]):
# Initialize the grasp object
g = Grasp()
# Set the pre-grasp and grasp postures appropriately;
# grasp_opening should be a bit smaller than target width
g.pre_grasp_posture = self.make_gripper_posture(self.gripper_opened)
g.grasp_posture = self.make_gripper_posture(grasp_opening)
# Set the approach and retreat parameters as desired
g.pre_grasp_approach = self.make_gripper_translation(0.1, 0.15, [0.0, 0.0, -1.5])
g.post_grasp_retreat = self.make_gripper_translation(0.1, 0.15, [0.0, 0.0, 1.5])
# Set the first grasp pose to the input pose
g.grasp_pose = initial_pose_stamped
# Pitch angles to try
pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.4, -0.4, 0.5, -0.5, 0.6, -0.6, 0.7, -0.7, 0.8, -0.8, 0.9, -0.9, 1.0, -1.0, 1.1, -1.1, 1.2, -1.2, 1.3, -1.3, 1.4, -1.4, 1.5, -1.5]
# Yaw angles to try; given the limited dofs of turtlebot_arm, we must calculate the heading
# from arm base to the object to pick (first we must transform its pose to arm base frame)
target_pose_arm_ref = self._tf2_buff.transform(initial_pose_stamped, ARM_BASE_FRAME)
x = target_pose_arm_ref.pose.position.x
y = target_pose_arm_ref.pose.position.y
yaw_vals = [atan2(y, x) + inc for inc in [0, 0.1,-0.1]]
# A list to hold the grasps
grasps = []
# Generate a grasp for each pitch and yaw angle
for yaw in yaw_vals:
for pitch in pitch_vals:
# Create a quaternion from the Euler angles
q = quaternion_from_euler(0, pitch, yaw)
# Set the grasp pose orientation accordingly
g.grasp_pose.pose.orientation.x = q[0]
g.grasp_pose.pose.orientation.y = q[1]
g.grasp_pose.pose.orientation.z = q[2]
g.grasp_pose.pose.orientation.w = q[3]
# Set and id for this grasp (simply needs to be unique)
g.id = str(len(grasps))
# Set the allowed touch objects to the input list
g.allowed_touch_objects = allowed_touch_objects
# Don't restrict contact force
g.max_contact_force = 0
# Degrade grasp quality for increasing pitch angles
g.grasp_quality = 1.0 - abs(pitch)
# Append the grasp to the list
grasps.append(deepcopy(g))
# Return the list
return grasps
# Generate a list of possible place poses
def make_places(self, target_id, 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.005] #, 0.01, -0.01, 0.015, -0.015]
# A list of y shifts (meters) to try
y_vals = [0, 0.005, -0.005, 0.01, -0.01] #, 0.015, -0.015]
# A list of pitch angles to try
pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.4, -0.4, 0.5, -0.5, 0.6, -0.6, 0.7, -0.7, 0.8, -0.8, 0.9, -0.9, 1.0, -1.0, 1.1, -1.1, 1.2, -1.2, 1.3, -1.3, 1.4, -1.4, 1.5, -1.5] #, 0.005, -0.005, 0.01, -0.01, 0.02, -0.02]
# A list to hold the places
places = []
# Generate a place pose for each angle and translation
for pitch in pitch_vals:
for dy in y_vals:
for dx in x_vals:
place.pose.position.x = init_pose.pose.position.x + dx
place.pose.position.y = init_pose.pose.position.y + dy
# Yaw angle: given the limited dofs of turtlebot_arm, we must calculate the heading from
# arm base to the place location (first we must transform its pose to arm base frame)
target_pose_arm_ref = self._tf2_buff.transform(place, ARM_BASE_FRAME)
x = target_pose_arm_ref.pose.position.x
y = target_pose_arm_ref.pose.position.y
yaw = atan2(y, x)
# Create a quaternion from the Euler angles
q = quaternion_from_euler(0, pitch, yaw)
# 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]
# MoveGroup::place will transform the provided place pose with the attached body pose, so the object retains
# the orientation it had when picked. However, with our 4-dofs arm this is infeasible (nor we care about the
# objects orientation!), so we cancel this transformation. It is applied here:
# https://github.com/ros-planning/moveit_ros/blob/jade-devel/manipulation/pick_place/src/place.cpp#L64
# More details on this issue: https://github.com/ros-planning/moveit_ros/issues/577
acobjs = self.scene.get_attached_objects([target_id])
aco_pose = self.get_pose(acobjs[target_id])
if aco_pose is None:
rospy.logerr("Attached collision object '%s' not found" % target_id)
return None
aco_tf = self.pose_to_mat(aco_pose)
place_tf = self.pose_to_mat(place.pose)
place.pose = self.mat_to_pose(place_tf, aco_tf)
rospy.logdebug("Compensate place pose with the attached object pose [%s]. Results: [%s]" \
% (aco_pose, place.pose))
# Append this place pose to the list
places.append(deepcopy(place))
# Return the list
return places
# 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 get_pose(self, co):
# We get object's pose from the mesh/primitive poses; try first with the meshes
if isinstance(co, CollisionObject):
if co.mesh_poses:
return co.mesh_poses[0]
elif co.primitive_poses:
return co.primitive_poses[0]
else:
rospy.logerr("Collision object '%s' has no mesh/primitive poses" % co.id)
return None
elif isinstance(co, AttachedCollisionObject):
if co.object.mesh_poses:
return co.object.mesh_poses[0]
elif co.object.primitive_poses:
return co.object.primitive_poses[0]
else:
rospy.logerr("Attached collision object '%s' has no mesh/primitive poses" % co.id)
return None
else:
rospy.logerr("Input parameter is not a collision object")
return None
def pose_to_mat(self, pose):
'''Convert a pose message to a 4x4 numpy matrix.
Args:
pose (geometry_msgs.msg.Pose): Pose rospy message class.
Returns:
mat (numpy.matrix): 4x4 numpy matrix
'''
quat = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]
pos = np.matrix([pose.position.x, pose.position.y, pose.position.z]).T
mat = np.matrix(quaternion_matrix(quat))
mat[0:3, 3] = pos
return mat
def mat_to_pose(self, mat, transform=None):
'''Convert a homogeneous matrix to a Pose message, optionally premultiply by a transform.
Args:
mat (numpy.ndarray): 4x4 array (or matrix) representing a homogenous transform.
transform (numpy.ndarray): Optional 4x4 array representing additional transform
Returns:
pose (geometry_msgs.msg.Pose): Pose message representing transform.
'''
if transform != None:
mat = np.dot(mat, transform)
pose = Pose()
pose.position.x = mat[0,3]
pose.position.y = mat[1,3]
pose.position.z = mat[2,3]
quat = quaternion_from_matrix(mat)
pose.orientation.x = quat[0]
pose.orientation.y = quat[1]
pose.orientation.z = quat[2]
pose.orientation.w = quat[3]
return pose
class GraspingClient(object):
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.move_group = MoveGroupInterface("arm", "base_link")
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
def updateScene(self):
# find objects
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
find_result = self.find_client.get_result()
# 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()
# insert objects to scene
idx = -1
for obj in find_result.objects:
idx += 1
obj.object.name = "object%d"%idx
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
wait = False)
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0],
1.5, # wider
obj.primitives[0].dimensions[2] + height]
obj.primitive_poses[0].position.z += -height/2.0
# add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
wait = False)
self.scene.waitForSync()
# store for grasping
self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
def getGraspableCube(self):
graspable = None
for obj in self.objects:
# need grasps
if len(obj.grasps) < 1:
continue
# check size
if obj.object.primitives[0].dimensions[0] < 0.05 or \
obj.object.primitives[0].dimensions[0] > 0.07 or \
obj.object.primitives[0].dimensions[0] < 0.05 or \
obj.object.primitives[0].dimensions[0] > 0.07 or \
obj.object.primitives[0].dimensions[0] < 0.05 or \
obj.object.primitives[0].dimensions[0] > 0.07:
continue
# has to be on table
if obj.object.primitive_poses[0].position.z < 0.5:
continue
return obj.object, obj.grasps
# nothing detected
return None, None
def getSupportSurface(self, name):
for surface in self.support_surfaces:
if surface.name == name:
return surface
return None
def getPlaceLocation(self):
pass
def pick(self, block, grasps):
success, pick_result = self.pickplace.pick_with_retry(block.name,
grasps,
support_name=block.support_surface,
scene=self.scene)
self.pick_result = pick_result
return success
def place(self, block, pose_stamped):
places = list()
l = PlaceLocation()
l.place_pose.pose = pose_stamped.pose
l.place_pose.header.frame_id = pose_stamped.header.frame_id
# copy the posture, approach and retreat from the grasp used
l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach
l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
places.append(copy.deepcopy(l))
# create another several places, rotate each by 360/m degrees in yaw direction
m = 16 # number of possible place poses
pi = 3.141592653589
for i in range(0, m-1):
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 2 * pi / m)
places.append(copy.deepcopy(l))
success, place_result = self.pickplace.place_with_retry(block.name,
places,
scene=self.scene)
return success
def tuck(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
class GraspingClient(object):
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.move_group = MoveGroupInterface("arm", "base_link")
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(
find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
# #######################################################################
# # test fetch_grasp_planner_node/plan with pose subcriber
# # use fetch_grasp_planner_node/plan with fetch_grasp_planner_node/plan
# grasp_topic = "fetch_grasp_planner_node/plan"
# rospy.loginfo("Waiting for %s..." % grasp_topic)
# self.grasp_planner_client = actionlib.SimpleActionClient(grasp_topic, GraspPlanningAction)
# self.grasp_planner_client.wait_for_server()
# #######################################################################
# #####################################################################
# # test fetch_grasp_planner_node/plan with pose subcriber
# rospy.Subscriber("perception/apple_pose", Pose, self.apple_pose_callback)
# self.object = Object()
# self.grasps = Grasp()
# def apple_pose_callback(self, message):
# apple = SolidPrimitive()
# apple.type = SolidPrimitive.SPHERE
# apple.dimensions = 0.04
# self.object.primitives = apple
# self.object.primitive_poses = message
# # add stamp and frame
# self.object.header.stamp = rospy.Time.now()
# self.object.header.frame_id = message.frame_id
# goal = GraspPlanningGoal()
# goal.object = self.object
# self.grasp_planner_client.send_goal(goal)
# self.grasp_planner_client.wait_for_result(rospy.Duration(5.0))
# grasp_planner_result = self.grasp_planner_client.get_result()
# self.grasps = grasp_planner_result.grasps
# ####################################################################
def updateScene(self):
# ####################################################################
# # use fetch_grasp_planner_node/plan with fetch_grasp_planner_node/plan
# # find objects
# goal = FindGraspableObjectsGoal()
# goal.plan_grasps = False
# self.find_client.send_goal(goal)
# self.find_client.wait_for_result(rospy.Duration(5.0))
# find_result = self.find_client.get_result()
# for obj in find_result.objects:
# goal = GraspPlanningGoal()
# goal.object = obj.object
# self.grasp_planner_client.send_goal(goal)
# self.grasp_planner_client.wait_for_result(rospy.Duration(5.0))
# grasp_planner_result = self.grasp_planner_client.get_result()
# obj.grasps = grasp_planner_result.grasps
# ####################################################################
# find objects
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
find_result = self.find_client.get_result()
# 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()
# insert objects to scene
objects = list()
idx = -1
for obj in find_result.objects:
idx += 1
obj.object.name = "object%d" % idx
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
use_service=False)
if obj.object.primitive_poses[0].position.x < 0.85:
objects.append([obj, obj.object.primitive_poses[0].position.z])
print("object ", idx, " frame_id = ", obj.object.header.frame_id)
print("object ", idx, " pose = ", obj.object.primitive_poses[0])
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [
obj.primitives[0].dimensions[0],
1.5, # wider
obj.primitives[0].dimensions[2] + height
]
obj.primitive_poses[0].position.z += -height / 2.0
# add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
use_service=False)
self.scene.waitForSync()
# store for grasping
#self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
# store graspable objects by Z
objects.sort(key=lambda object: object[1])
objects.reverse()
self.objects = [object[0] for object in objects]
#for object in objects:
# print(object[0].object.name, object[1])
#exit(-1)
def getGraspableObject(self):
graspable = None
for obj in self.objects:
# need grasps
if len(obj.grasps) < 1:
continue
# check size
if obj.object.primitives[0].dimensions[0] < 0.03 or \
obj.object.primitives[0].dimensions[0] > 0.25 or \
obj.object.primitives[0].dimensions[0] < 0.03 or \
obj.object.primitives[0].dimensions[0] > 0.25 or \
obj.object.primitives[0].dimensions[0] < 0.03 or \
obj.object.primitives[0].dimensions[0] > 0.25:
continue
# has to be on table
if obj.object.primitive_poses[0].position.z < 0.5:
continue
print obj.object.primitive_poses[0], obj.object.primitives[0]
return obj.object, obj.grasps
# nothing detected
return None, None
def getSupportSurface(self, name):
for surface in self.support_surfaces:
if surface.name == name:
return surface
return None
def getPlaceLocation(self):
pass
def pick(self, block, grasps):
success, pick_result = self.pickplace.pick_with_retry(block.name,
grasps,
scene=self.scene)
self.pick_result = pick_result
return success
def place(self, block, pose_stamped):
places = list()
l = PlaceLocation()
l.place_pose.pose = pose_stamped.pose
l.place_pose.header.frame_id = pose_stamped.header.frame_id
# copy the posture, approach and retreat from the grasp used
l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach
l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
places.append(copy.deepcopy(l))
# create another several places, rotate each by 360/m degrees in yaw direction
m = 16 # number of possible place poses
pi = 3.141592653589
for i in range(0, m - 1):
l.place_pose.pose = rotate_pose_msg_by_euler_angles(
l.place_pose.pose, 0, 0, 2 * pi / m)
places.append(copy.deepcopy(l))
success, place_result = self.pickplace.place_with_retry(
block.name, places, scene=self.scene)
return success
def tuck(self):
joints = [
"shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def stow(self):
joints = [
"shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
pose = [1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def intermediate_stow(self):
joints = [
"shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
pose = [0.7, -0.3, 0.0, -0.3, 0.0, -0.57, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return