def create_placings_from_object_pose(self, posestamped):
""" Create a list of PlaceLocation of the object rotated every 15deg"""
place_locs = []
pre_grasp_posture = JointTrajectory()
# Actually ignored....
pre_grasp_posture.header.frame_id = self._grasp_pose_frame_id
pre_grasp_posture.joint_names = [
name for name in self._gripper_joint_names.split()]
jtpoint = JointTrajectoryPoint()
jtpoint.positions = [
float(pos) for pos in self._gripper_pre_grasp_positions.split()]
jtpoint.time_from_start = rospy.Duration(self._time_pre_grasp_posture)
pre_grasp_posture.points.append(jtpoint)
# Generate all the orientations every step_degrees_yaw deg
for yaw_angle in np.arange(0.0, 2.0 * pi, radians(self._step_degrees_yaw)):
pl = PlaceLocation()
pl.place_pose = posestamped
newquat = quaternion_from_euler(0.0, 0.0, yaw_angle)
pl.place_pose.pose.orientation = Quaternion(
newquat[0], newquat[1], newquat[2], newquat[3])
# TODO: the frame is ignored, this will always be the frame of the gripper
# so arm_tool_link
pl.pre_place_approach = self.createGripperTranslation(
Vector3(1.0, 0.0, 0.0))
pl.post_place_retreat = self.createGripperTranslation(
Vector3(-1.0, 0.0, 0.0))
pl.post_place_posture = pre_grasp_posture
place_locs.append(pl)
return place_locs
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 createPlaceLocations(posestamped, deg_step=15):
"""Create a list of PlaceLocation of the object rotated every deg_step, defaults to 15 degrees"""
place_locs = []
for yaw_angle in np.arange(0, 2 * pi, radians(deg_step)):
pl = PlaceLocation()
pl.place_pose = posestamped
newquat = quaternion_from_euler(0.0, 0.0, yaw_angle)
pl.place_pose.pose.orientation = Quaternion(newquat[0], newquat[1], newquat[2], newquat[3])
pl.pre_place_approach = createGripperTranslation(Vector3(0.0, 0.0, -1.0))
pl.post_place_retreat = createGripperTranslation(Vector3(0.0, 0.0, 1.0))
pl.post_place_posture = getPreGraspPosture()
place_locs.append(pl)
return place_locs
def generate_place_locations( position):
pls = []
pl = PlaceLocation()
pl.place_pose = position
for x in range(0, 360, 2):
quat = quaternion_from_euler(0.0, 0.0, round(math.radians(x), 4))
pl.place_pose.pose.orientation = Quaternion(*quat)
# take object along negative z-axis
pl.pre_place_approach = create_gripper_translation(Vector3(0.0, 0.0, -1.0))
# go with object along positive z-axis
pl.post_place_retreat = create_gripper_translation(Vector3(0.0, 0.0, 1.0))
pl.post_place_posture = get_post_place_posture()
pls.append(copy.deepcopy(pl))
return pls
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 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 make_places(self,
init_pose,
allowed_touch_objects,
pre,
post,
roll_vals=[0],
pitch_vals=[0],
yaw_vals=[0]):
place = PlaceLocation()
place.post_place_posture = self.make_gripper_posture(GRIPPER_OPEN)
place.pre_place_approach = self.make_gripper_translation(
pre[0], pre[1], pre[2])
place.post_place_retreat = self.make_gripper_translation(
post[0], post[1], post[2])
place.place_pose = init_pose
x_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015]
y_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015]
places = []
for yaw in yaw_vals:
for pitch in pitch_vals:
for y in y_vals:
for x in x_vals:
place.place_pose.pose.position.x = init_pose.pose.position.x + x
place.place_pose.pose.position.y = init_pose.pose.position.y + y
q = quaternion_from_euler(0, pitch, yaw)
place.place_pose.pose.orientation.x = q[0]
place.place_pose.pose.orientation.y = q[1]
place.place_pose.pose.orientation.z = q[2]
place.place_pose.pose.orientation.w = q[3]
place.id = str(len(places))
place.allowed_touch_objects = allowed_touch_objects
places.append(deepcopy(place))
return places
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 90 degrees in yaw direction
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)
places.append(copy.deepcopy(l))
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)
places.append(copy.deepcopy(l))
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)
places.append(copy.deepcopy(l))
success, place_result = self.pickplace.place_with_retry(block.name,
places,
scene=self.scene)
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 90 degrees in yaw direction
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)
places.append(copy.deepcopy(l))
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)
places.append(copy.deepcopy(l))
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)
places.append(copy.deepcopy(l))
success, place_result = self.pickplace.place_with_retry(block.name,
places,
scene=self.scene)
return success
def generate_place_poses(self, initial_place_pose):
places = list()
l = PlaceLocation()
l.id = "dupadupa"
l.place_pose.header.frame_id = "camera_color_optical_frame"
q = Quaternion(initial_place_pose.grasp_pose.pose.orientation.w,
initial_place_pose.grasp_pose.pose.orientation.x,
initial_place_pose.grasp_pose.pose.orientation.y,
initial_place_pose.grasp_pose.pose.orientation.z)
# Load successful grasp pose
l.place_pose.pose.position = initial_place_pose.grasp_pose.pose.position
l.place_pose.pose.orientation.w = q.elements[0]
l.place_pose.pose.orientation.x = q.elements[1]
l.place_pose.pose.orientation.y = q.elements[2]
l.place_pose.pose.orientation.z = q.elements[3]
# Move 20cm to the right
l.place_pose.pose.position.y += 0.2
# Fill rest of the msg with some data
l.post_place_posture = initial_place_pose.grasp_posture
l.post_place_retreat = initial_place_pose.post_grasp_retreat
l.pre_place_approach = initial_place_pose.pre_grasp_approach
places.append(copy.deepcopy(l))
# Rotate place pose to generate more possible configurations for the planner
m = 16 # Number of possible place poses
for i in range(0, m - 1):
l.place_pose.pose = rotate_pose_msg_by_euler_angles(
l.place_pose.pose, 0, 0, 2 * math.pi / m)
places.append(copy.deepcopy(l))
return places
def make_places(self, pose_stamped, mega_angle=False):
# setup default of place location
l = PlaceLocation()
l.post_place_posture = self.make_gripper_posture(GRIPPER_OPEN)
l.pre_place_approach = self.make_gripper_translation(0.1, 0.15)
l.post_place_retreat = self.make_gripper_translation(0.1, 0.15, -1.0)
l.place_pose = pose_stamped
pitch_vals = [0, 0.2, -0.2, 0.4, -0.4]
if mega_angle:
pitch_vals += [0.3, -0.3, 0.5, -0.5, 0.6, -0.6]
# generate list of place locations
places = []
for y in [-1.57, -0.78, 0, 0.78, 1.57]:
for p in pitch_vals:
q = quaternion_from_euler(0, p, y) # now in object frame
l.place_pose.pose.orientation.x = q[0]
l.place_pose.pose.orientation.y = q[1]
l.place_pose.pose.orientation.z = q[2]
l.place_pose.pose.orientation.w = q[3]
l.id = str(len(places))
places.append(copy.deepcopy(l))
return places
def generate_place_locations(self, goal_pose):
pls = []
# get euler axis values
axis = euler_from_quaternion([goal_pose.pose.orientation.x, goal_pose.pose.orientation.y,
goal_pose.pose.orientation.z, goal_pose.pose.orientation.w])
tolerance = round(math.pi/3, 4)
step = 0.01
z_axis_angle = round(axis[2], 4)
# create a range of possible place locations
for x in arange(round((z_axis_angle - tolerance), 0), round((z_axis_angle + tolerance), 0), step, dtype=float):
# for x in arange(0, 360, step, dtype=float):
pl = PlaceLocation()
pl.place_pose = goal_pose
quat = quaternion_from_euler(0.0, 0.0, round(x, 4))
pl.place_pose.pose.orientation = Quaternion(*quat)
# gripper comes from negative z axis
pl.pre_place_approach = self.create_gripper_translation(Vector3(0.0, 0.0, -1.0))
# gripper leaves place position on positive z axis
pl.post_place_retreat = self.create_gripper_translation(Vector3(0.0, 0.0, 1.0))
# gripper joint values
pl.post_place_posture = self.get_post_place_posture()
pls.append(copy.deepcopy(pl))
return pls
rospy.loginfo("Beginning to pick.")
success, pick_result = pickplace.pick_with_retry(obj_name, grasps, support_name=support_surface, scene=scene)
if not success:
exit(-1)
# create a set of place locations for the cube
places = list()
l = PlaceLocation()
l.place_pose.pose = find_result.objects[the_object].object.primitive_poses[0]
l.place_pose.header.frame_id = find_result.objects[the_object].object.header.frame_id
# invert the y of the pose
l.place_pose.pose.position.y = -l.place_pose.pose.position.y
# copy the posture, approach and retreat from the grasp used
l.post_place_posture = pick_result.grasp.pre_grasp_posture
l.pre_place_approach = pick_result.grasp.pre_grasp_approach
l.post_place_retreat = pick_result.grasp.post_grasp_retreat
places.append(copy.deepcopy(l))
# create another several places, rotate each by 90 degrees in yaw direction
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)
places.append(copy.deepcopy(l))
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)
places.append(copy.deepcopy(l))
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)
places.append(copy.deepcopy(l))
# drop it like it's hot
rospy.loginfo("Beginning to place.")
while not rospy.is_shutdown():
# can't fail here or moveit needs to be restarted
success, place_result = pickplace.place_with_retry(obj_name, places, support_name=support_surface, scene=scene)
if success:
def __init__(self):
rospy.init_node('urb_pickup', anonymous=False)
rospy.on_shutdown(self.shutdown)
# Get Goal point where we pick up
goal_x = rospy.get_param("~goal_x", 3.96) # meters
goal_y = rospy.get_param("~goal_y", 4.01)
goal_z = rospy.get_param("~goal_z", pi/2)
parser = argparse.ArgumentParser(description="Simple demo of pick and place")
parser.add_argument("--objects", help="Just do object perception", action="store_true")
parser.add_argument("--all", help="Just do object perception, but insert all objects", action="store_true")
parser.add_argument("--once", help="Run once.", action="store_true")
parser.add_argument("--ready", help="Move the arm to the ready position.", action="store_true")
parser.add_argument("--plan", help="Only do planning, no execution", action="store_true")
parser.add_argument("--x", help="Recommended x offset, how far out an object should roughly be.", type=float, default=0.5)
args, unknown = parser.parse_known_args()
move_group = MoveGroupInterface("arm", "base_link", plan_only = args.plan)
# Create a list to hold the target quaternions (orientations)
quaternions = list()
# Create a list to hold the waypoint poses
waypoints = list()
q_angle = quaternion_from_euler(0, 0, goal_z, axes='sxyz')
q = Quaternion(*q_angle)
quaternions.append(q)
waypoints.append(Pose(Point(goal_x, goal_y, 0.0), quaternions[0]))
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
rospy.loginfo("Starting navigation test")
# Intialize the waypoint goal
goal = MoveBaseGoal()
# Use the map frame to define goal poses
goal.target_pose.header.frame_id = 'map'
# Set the time stamp to "now"
goal.target_pose.header.stamp = rospy.Time.now()
# Set the goal pose to the i-th waypoint
goal.target_pose.pose = waypoints[0]
# Start the robot moving toward the goal
self.move(goal)
# if all we want to do is prepare the arm, do it now
if args.ready:
self.move_to_ready(move_group)
exit(0)
scene = PlanningSceneInterface("base_link")
pickplace = PickPlaceInterface("arm", "gripper", plan_only = args.plan, verbose = True)
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")
while not rospy.is_shutdown():
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
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))
# remove all previous objects
for name in scene.getKnownCollisionObjects():
scene.removeCollisionObject(name, False)
for name in scene.getKnownAttachedObjects():
scene.removeAttachedObject(name, False)
scene.waitForSync()
# clear colors
scene._colors = dict()
# insert objects, find the one to grasp
the_object = None
the_object_dist = 0.35
count = -1
for obj in find_result.objects:
count += 1
scene.addSolidPrimitive("object%d"%count,
obj.object.primitives[0],
obj.object.primitive_poses[0],
wait = False)
# object must have usable grasps
if len(obj.grasps) < 1:
continue
# choose object in front of 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
if the_object == None:
rospy.logerr("Nothing to grasp! try again...")
continue
# insert table
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)
obj_name = "object%d"%the_object
# sync
scene.waitForSync()
# set color of object we are grabbing
scene.setColor(obj_name, 223.0/256.0, 90.0/256.0, 12.0/256.0) # orange
scene.setColor(find_result.objects[the_object].object.support_surface, 0, 0, 0) # black
scene.sendColors()
# exit now if we are just doing object update
if args.objects:
if args.once:
exit(0)
else:
continue
# get grasps (we checked that they exist above)
grasps = find_result.objects[the_object].grasps
support_surface = find_result.objects[the_object].object.support_surface
# call move_group to pick the object
rospy.loginfo("Beginning to pick.")
success, pick_result = pickplace.pick_with_retry(obj_name, grasps, support_name=support_surface, scene=scene)
if not success:
exit(-1)
# create a set of place locations for the cube
places = list()
l = PlaceLocation()
l.place_pose.pose = find_result.objects[the_object].object.primitive_poses[0]
l.place_pose.header.frame_id = find_result.objects[the_object].object.header.frame_id
# invert the y of the pose
l.place_pose.pose.position.y = -l.place_pose.pose.position.y
# copy the posture, approach and retreat from the grasp used
l.post_place_posture = pick_result.grasp.pre_grasp_posture
l.pre_place_approach = pick_result.grasp.pre_grasp_approach
l.post_place_retreat = pick_result.grasp.post_grasp_retreat
places.append(copy.deepcopy(l))
# create another several places, rotate each by 90 degrees in yaw direction
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)
places.append(copy.deepcopy(l))
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)
places.append(copy.deepcopy(l))
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)
places.append(copy.deepcopy(l))
# drop it like it's hot
rospy.loginfo("Beginning to place.")
while not rospy.is_shutdown():
# can't fail here or moveit needs to be restarted
success, place_result = pickplace.place_with_retry(obj_name, places, support_name=support_surface, scene=scene)
if success:
break
# place arm back at side
self.move_to_ready(move_group)
rospy.loginfo("Ready...")
# rinse and repeat
if args.once:
exit(0)
rospy.loginfo("Beginning to pick.")
success, pick_result = pickplace.pick_with_retry(obj_name, grasps, support_name=support_surface, scene=scene)
if not success:
exit(-1)
# create a set of place locations for the cube
places = list()
l = PlaceLocation()
l.place_pose.pose = find_result.objects[the_object].object.primitive_poses[0]
l.place_pose.header.frame_id = find_result.objects[the_object].object.header.frame_id
# invert the y of the pose
l.place_pose.pose.position.y = -l.place_pose.pose.position.y
# copy the posture, approach and retreat from the grasp used
l.post_place_posture = pick_result.grasp.pre_grasp_posture
l.pre_place_approach = pick_result.grasp.pre_grasp_approach
l.post_place_retreat = pick_result.grasp.post_grasp_retreat
places.append(copy.deepcopy(l))
# create another several places, rotate each by 90 degrees in yaw direction
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)
places.append(copy.deepcopy(l))
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)
places.append(copy.deepcopy(l))
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 1.57)
places.append(copy.deepcopy(l))
# drop it like it's hot
rospy.loginfo("Beginning to place.")
while not rospy.is_shutdown():
# can't fail here or moveit needs to be restarted
success, place_result = pickplace.place_with_retry(obj_name, places, support_name=support_surface, scene=scene)
if success:
