def place(self, block, pose_stamped, placePos):
#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 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 place(self, block, pose_stamped, gripper=0):
places = list()
loc = PlaceLocation()
loc.place_pose.pose = pose_stamped.pose
loc.place_pose.header.frame_id = pose_stamped.header.frame_id
# copy the posture, approach and retreat from the grasp used
loc.post_place_posture = self.pick_result[
gripper].grasp.pre_grasp_posture
loc.pre_place_approach = self.pick_result[
gripper].grasp.pre_grasp_approach
loc.post_place_retreat = self.pick_result[
gripper].grasp.post_grasp_retreat
places.append(copy.deepcopy(loc))
# 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):
loc.place_pose.pose = rotate_pose_msg_by_euler_angles(
loc.place_pose.pose, 0, 0, 2 * pi / m)
places.append(copy.deepcopy(loc))
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 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 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(robot):
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0.7
p.pose.position.y = 0.0
p.pose.position.z = 0.5
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1.0
g = PlaceLocation()
g.place_pose = p
g.pre_place_approach.direction.vector.z = -1.0
g.post_place_retreat.direction.vector.x = -1.0
g.post_place_retreat.direction.header.frame_id = robot.get_planning_frame()
g.pre_place_approach.direction.header.frame_id = "r_wrist_roll_link"
g.pre_place_approach.min_distance = 0.1
g.pre_place_approach.desired_distance = 0.2
g.post_place_retreat.min_distance = 0.1
g.post_place_retreat.desired_distance = 0.25
g.post_place_posture = open_gripper(g.post_place_posture)
group = robot.get_group("right_arm")
group.set_support_surface_name("table")
# Add path constraints
constr = Constraints()
constr.orientation_constraints = []
ocm = OrientationConstraint()
ocm.link_name = "r_wrist_roll_link"
ocm.header.frame_id = p.header.frame_id
ocm.orientation.x = 0.0
ocm.orientation.y = 0.0
ocm.orientation.z = 0.0
ocm.orientation.w = 1.0
ocm.absolute_x_axis_tolerance = 0.2
ocm.absolute_y_axis_tolerance = 0.2
ocm.absolute_z_axis_tolerance = math.pi
ocm.weight = 1.0
constr.orientation_constraints.append(ocm)
# group.set_path_constraints(constr)
group.set_planner_id("RRTConnectkConfigDefault")
group.place("part", g)
def place(pickPlaceInterface, frame, placePose_xyzrpy, objectName, supportName):
moveit_place = PlaceLocation()
moveit_place.place_pose.header.frame_id = frame
moveit_place.place_pose.pose.orientation = geometry_msgs.msg.Quaternion(
*transformations.quaternion_from_euler(*placePose_xyzrpy[3:])
)
moveit_place.place_pose.pose.position.x = placePose_xyzrpy[0]
moveit_place.place_pose.pose.position.y = placePose_xyzrpy[1]
moveit_place.place_pose.pose.position.z = placePose_xyzrpy[2]
moveit_place.pre_place_approach.direction.header.frame_id = frame
moveit_place.pre_place_approach.direction.vector.z = -1.0
moveit_place.pre_place_approach.min_distance = 0.095
moveit_place.pre_place_approach.desired_distance = 0.115
moveit_place.post_place_retreat.direction.header.frame_id = frame
moveit_place.post_place_retreat.direction.vector.x = -1.0
moveit_place.post_place_retreat.min_distance = 0.095
moveit_place.post_place_retreat.desired_distance = 0.115
openGripper(moveit_place.post_place_posture)
for i in range(10):
res = pickPlaceInterface.place(objectName, [moveit_place, ], support_name=supportName, goal_is_eef=True)
# print("{}: {}".format(i, res.error_code.val))
if res.error_code.val == 1:
return True
return False
def make_location(arm,
gripper,
place_pose,
descent_distance=0.2,
retreat_distance=0.2):
"""Initialize the moveit_msgs/Grasp message.
Arguments:
arm {MoveGroupCommander} -- arm group
gripper {MoveGroupCommander} -- gripper group
place_pose {PoseStamped} -- target object pose
Keyword Arguments:
descent_distance {float} -- descent distance to take before placing an object (default: {0.2})
retreat_distance {float} -- retreat distance to take after a place has been completed (default: {0.2})
Returns:
PlaceLocation -- message
"""
return PlaceLocation(id='grasp',
post_place_posture=_make_posture(
gripper.get_named_target_values('open')),
place_pose=place_pose,
pre_place_approach=_make_gripper_translation(
arm.get_planning_frame(), [0, 0, -1],
descent_distance),
post_place_retreat=_make_gripper_translation(
arm.get_end_effector_link(), [0, 0, -1],
retreat_distance),
allowed_touch_objects=[])
def place(self):
#create a place location msg
pl=PlaceLocation()
#Setting place location pose
# +++++++++++++++++++++++++++
place_pose = PoseStamped()
place_pose.header.frame_id = "panda_link0"
place_pose.pose.position.x = 0
place_pose.pose.position.y = 0.5
place_pose.pose.position.z = 0.5
#orientation in quaternion
q = quaternion_from_euler(0,0,pi/2)
place_pose.pose.orientation.x = q[0]
place_pose.pose.orientation.y = q[1]
place_pose.pose.orientation.z = q[2]
place_pose.pose.orientation.w = q[3]
pl.place_pose=place_pose #set place location
# Setting pre-place approach
# ++++++++++++++++++++++++++
#* Defined with respect to frame_id */
pl.pre_place_approach.direction.header.frame_id = "panda_link0"
# Direction is set as negative z axis */
pl.pre_place_approach.direction.vector.z = -1.0
pl.pre_place_approach.min_distance = 0.095
pl.pre_place_approach.desired_distance = 0.115
# Setting post-grasp retreat
# ++++++++++++++++++++++++++
# Defined with respect to frame_id */
pl.post_place_retreat.direction.header.frame_id = "panda_link0"
#* Direction is set as negative y axis */
pl.post_place_retreat.direction.vector.y = -1.0
pl.post_place_retreat.min_distance = 0.1
pl.post_place_retreat.desired_distance = 0.25
# Setting posture of eef after placing object
# +++++++++++++++++++++++++++++++++++++++++++
#* Similar to the pick case */
self.openGripperPose(pl.post_place_posture)
# Set support surface as table2.
self.move_group_arm.set_support_surface_name("table2")
# Call place to place the object using the place locations given.
self.move_group_arm.place("box", pl)
def generate_place_locations(position):
pls = []
pl = PlaceLocation()
pl.place_pose.pose.position = 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 _generate_places(self, target):
"""
Generate places (place locations), based on
https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/
baxter_pick_place/src/block_pick_place.cpp
Create a Pose Array data for the pose of the place
"""
# Generate places:
places = []
now = rospy.Time.now()
for angle in numpy.arange(0.0, numpy.deg2rad(360.0),
numpy.deg2rad(1.0)):
# Create place location:
place = PlaceLocation()
place.place_pose.header.stamp = now
place.place_pose.header.frame_id = self._robot.get_planning_frame()
# Set target position:
place.place_pose.pose = copy.deepcopy(target)
# Generate orientation (wrt Z axis):
q = quaternion_from_euler(0.0, 0.0, angle)
place.place_pose.pose.orientation = Quaternion(*q)
# Generate pre place approach:
place.pre_place_approach.desired_distance = self._approach_retreat_desired_dist
place.pre_place_approach.min_distance = self._approach_retreat_min_dist
place.pre_place_approach.direction.header.stamp = now
place.pre_place_approach.direction.header.frame_id = self._robot.get_planning_frame(
)
place.pre_place_approach.direction.vector.x = 0
place.pre_place_approach.direction.vector.y = 0
place.pre_place_approach.direction.vector.z = 0.2
# Generate post place approach:
place.post_place_retreat.direction.header.stamp = now
place.post_place_retreat.direction.header.frame_id = self._robot.get_planning_frame(
)
place.post_place_retreat.desired_distance = self._approach_retreat_desired_dist
place.post_place_retreat.min_distance = self._approach_retreat_min_dist
place.post_place_retreat.direction.vector.x = 0
place.post_place_retreat.direction.vector.y = 0
place.post_place_retreat.direction.vector.z = 0.2
# Add place:
places.append(place)
# Publish places (for debugging/visualization purposes):
self._publish_places(places)
return places
def place(group):
"""Place object.
Parameters
----------
Group : moveit_commander.RobotCommander
Moveit_commander move group.
"""
## - BEGIN_SUB_TUTORIAL place - ##
# TODO(@ridhwanluthra) - Calling place function may lead to "All supplied place locations failed. Retrying last
# location in
# verbose mode." This is a known issue and we are working on fixing it. |br|
# Create a vector of placings to be attempted, currently only creating single place location.
place_location = [PlaceLocation() for i in range(1)]
## Setting place location pose ##
place_location[0].place_pose.header.frame_id = "panda_link0"
orientation = quaternion_from_euler(0, 0, math.pi / 2)
place_location[0].place_pose.pose.orientation.x = orientation[0]
place_location[0].place_pose.pose.orientation.y = orientation[1]
place_location[0].place_pose.pose.orientation.z = orientation[2]
place_location[0].place_pose.pose.orientation.w = orientation[3]
## While placing it is the exact location of the center of the object. ##
place_location[0].place_pose.pose.position.x = 0
place_location[0].place_pose.pose.position.y = 0.5
place_location[0].place_pose.pose.position.z = 0.5
## Setting pre-place approach ##
# Defined with respect to frame_id
place_location[
0].pre_place_approach.direction.header.frame_id = "panda_link0"
place_location[0].pre_place_approach.direction.vector.z = (
-1.0) # Direction is set as negative z axis
place_location[0].pre_place_approach.min_distance = 0.095
place_location[0].pre_place_approach.desired_distance = 0.115
# Setting post-grasp retreat
# Defined with respect to frame_id
place_location[
0].post_place_retreat.direction.header.frame_id = "panda_link0"
place_location[0].post_place_retreat.direction.vector.y = (
-1.0) # Direction is set as negative y axis
place_location[0].post_place_retreat.min_distance = 0.1
place_location[0].post_place_retreat.desired_distance = 0.25
# Setting posture of eef after placing object
# Similar to the pick case
openGripper(place_location[0].post_place_posture)
## Set support surface as table2 ##
group.set_support_surface_name("table2")
## Call place to place the object using the place locations given. ##
group.place("object", place_location[0])
def place(self, place_msg):
pevent("Place sequence started")
#places = self.generate_place_poses(place_pose)
#place_pose is a Grasp msg
l = PlaceLocation()
l.id = "place target"
l.place_pose = place_msg.grasp_pose
l.place_pose.pose.position.y = -l.place_pose.pose.position.y
_, place_result = self.p.place_with_retry("obj", [
l,
],
support_name="<octomap>",
planning_time=9001,
goal_is_eef=True)
pevent("Planner returned: " +
get_moveit_error_code(place_result.error_code.val))
def generate_places(self, target):
#places = []
now = rospy.Time.now()
# for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(30.0)):
# Create place location:
place = PlaceLocation()
place.place_pose.header.stamp = now
place.place_pose.header.frame_id = self.robot.get_planning_frame()
# Set target position:
place.place_pose.pose = copy.deepcopy(target)
# Generate orientation (wrt Z axis):
# q = quaternion_from_euler(0.0, 0, 0.0)
# place.place_pose.pose.orientation = Quaternion(*q)
# Generate pre place approach:
place.pre_place_approach.desired_distance = self.approach_retreat_desired_dist
place.pre_place_approach.min_distance = self.approach_retreat_min_dist
place.pre_place_approach.direction.header.stamp = now
place.pre_place_approach.direction.header.frame_id = self.robot.get_planning_frame(
)
place.pre_place_approach.direction.vector.x = 0
place.pre_place_approach.direction.vector.y = 0
place.pre_place_approach.direction.vector.z = -0.2
# Generate post place approach:
place.post_place_retreat.direction.header.stamp = now
place.post_place_retreat.direction.header.frame_id = self.robot.get_planning_frame(
)
place.post_place_retreat.desired_distance = self.approach_retreat_desired_dist
place.post_place_retreat.min_distance = self.approach_retreat_min_dist
place.post_place_retreat.direction.vector.x = 0
place.post_place_retreat.direction.vector.y = 0
place.post_place_retreat.direction.vector.z = 0.2
place.allowed_touch_objects.append(self.object_name)
place.post_place_posture.joint_names.append("gripper_finger1_joint")
traj = JointTrajectoryPoint()
traj.positions.append(0)
#traj.effort.append(0)
traj.time_from_start = rospy.Duration.from_sec(1.0)
place.post_place_posture.points.append(traj)
# Add place:
#places.append(place)
return place
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
def generate_place_poses(self, initial_place_pose):
places = list()
l = PlaceLocation()
l.id = "dupadupa"
l.place_pose.header.frame_id = "summit_xl_base_footprint"
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 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 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, place_pose):
#create a place location msg
pl = PlaceLocation()
#Setting place location pose
# +++++++++++++++++++++++++++
place_pose_stamped = PoseStamped()
place_pose_stamped.header.frame_id = "panda_link0"
place_pose_stamped.pose = place_pose
pl.place_pose = place_pose_stamped #set place location
# Setting pre-place approach
# ++++++++++++++++++++++++++
#* Defined with respect to frame_id */
pl.pre_place_approach.direction.header.frame_id = "panda_link0"
# Direction is set as negative z axis */
pl.pre_place_approach.direction.vector.z = -1.0
pl.pre_place_approach.min_distance = 0.095
pl.pre_place_approach.desired_distance = 0.115
# Setting post-grasp retreat
# ++++++++++++++++++++++++++
# Defined with respect to frame_id */
pl.post_place_retreat.direction.header.frame_id = "panda_link0"
#* Direction is set as negative y axis */
pl.post_place_retreat.direction.vector.y = -1.0
pl.post_place_retreat.min_distance = 0.1
pl.post_place_retreat.desired_distance = 0.25
# Setting posture of eef after placing object
# +++++++++++++++++++++++++++++++++++++++++++
#* Similar to the pick case */
openGripperPose(pl.post_place_posture)
# Set support surface as table2.
self.move_group_arm.set_support_surface_name("table2")
# Call place to place the object using the place locations given.
self.move_group_arm.place("block", pl)
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_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
def place(self, block, x, y, z, roll, pitch, yaw):
places = list()
#creates a pose for the end place of postion x,y,z and oreintation x,y,z,w made from an euler angle conversion
placePos = PlaceLocation()
placePos.place_pose.pose.orientation = geometry_msgs.msg.Quaternion(
*tf_conversions.transformations.quaternion_from_euler(
roll, pitch, yaw))
placePos.place_pose.pose.position.x = x
placePos.place_pose.pose.position.y = y
placePos.place_pose.pose.position.z = z
placePos.place_pose.header.frame_id = pose_stamped.header.frame_id
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 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 setup_scene():
scene = PlanningSceneInterface('base_link')
scene.removeCollisionObject('box')
scene.addCube('box', 0.25, -0.45, 0.1, 0.125)
pick_place = PickPlaceInterface('xarm5', 'gripper', verbose=True)
grasp = Grasp()
# fill in g
# setup object named object_name using PlanningSceneInterface
pick_place.pickup('box', [grasp], support_name='supporting_surface')
place_loc = PlaceLocation()
# fill in l
pick_place.place('box'[place_loc],
goal_is_eef=True,
support_name='supporting_surface')
def make_places(self,
init_pose,
allowed_touch_objects,
pre,
post,
set_rpy=0,
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)
if set_rpy:
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, current_goal):
# get the move group name.
move_group = self.move_group
# get the move group current position.
current_position = move_group.get_current_pose()
# calculate the quanternion from roll, pitch, yaw.
orientation = quaternion_from_euler(math.radians(
current_goal[3]), math.radians(current_goal[4]), math.radians(current_goal[5]))
# call place location class from moveit
place_location = PlaceLocation()
# Setting place location position and orientation
place_location.place_pose.header.frame_id = "panda_link0"
place_location.place_pose.pose.orientation.x = orientation[0]
place_location.place_pose.pose.orientation.y = orientation[1]
place_location.place_pose.pose.orientation.z = orientation[2]
place_location.place_pose.pose.orientation.w = orientation[3]
place_location.place_pose.pose.position.x = current_goal[0]
place_location.place_pose.pose.position.y = current_goal[1]
place_location.place_pose.pose.position.z = current_goal[2]
# Setting pre-place approach ##
place_location.pre_place_approach.direction.header.frame_id = "panda_link0"
place_location.pre_place_approach.direction.vector.z = place_location.place_pose.pose.position.z - current_position.pose.position.z # Direction is set as negative z axis
place_location.pre_place_approach.min_distance = 0.02
place_location.pre_place_approach.desired_distance = 0.3
# Setting post-place retreat
place_location.post_place_retreat.direction.header.frame_id = "panda_link0"
place_location.post_place_retreat.direction.vector.y = -1.0 # Direction is set as negative y axis
place_location.post_place_retreat.direction.vector.z = 0.8
place_location.post_place_retreat.min_distance = 0.1
place_location.post_place_retreat.desired_distance = 0.45
self.openGripper(place_location.post_place_posture)
move_group.set_support_surface_name(current_goal[6])
move_group.place(current_goal[7], place_location)
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 get_place_locations(self):
locations = []
for xx in np.arange(0.4, 0.5, 0.1):
for yy in np.arange(0.8, 0.9, 0.1):
pl = PlaceLocation()
pt = JointTrajectoryPoint()
pt.positions = [0.5]
pl.post_place_posture.points.append(pt)
pl.post_place_posture.joint_names = [
'l_gripper_motor_screw_joint'
]
pl.place_pose.header.frame_id = "odom_combined"
pl.place_pose.header.stamp = rospy.Time.now()
pl.place_pose.pose.position.x = xx
pl.place_pose.pose.position.y = yy
pl.place_pose.pose.position.z = 1.05
# Quaternion obtained by pointing wrist down
#pl.place_pose.pose.orientation.w = 1.0
pl.place_pose.pose.orientation.x = 0.761
pl.place_pose.pose.orientation.y = -0.003
pl.place_pose.pose.orientation.z = -0.648
pl.place_pose.pose.orientation.w = 0.015
pl.pre_place_approach.direction.header.frame_id = "odom_combined"
pl.pre_place_approach.direction.vector.z = -1.0
pl.pre_place_approach.desired_distance = 0.2
pl.pre_place_approach.min_distance = 0.1
pl.post_place_retreat.direction.header.frame_id = "odom_combined"
pl.post_place_retreat.direction.vector.z = 1.0
pl.post_place_retreat.desired_distance = 0.2
pl.post_place_retreat.min_distance = 0.1
locations.append(pl)
return locations
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
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)
def __init__(self):
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
self.arm = MoveGroupCommander("arm")
rospy.sleep(1)
#remove existing objects
self.scene.remove_world_object()
self.scene.remove_attached_object("endeff", "part")
rospy.sleep(5)
'''
# publish a demo scene
self.pos = PoseStamped()
self.pos.header.frame_id = "world"
# add wall in between
self.pos.pose.position.x = -0.14
self.pos.pose.position.y = 0.02
self.pos.pose.position.z = 0.09
self.scene.add_box("wall", pos, (0.06, 0.01, 0.18))
# add an object to be grasped
self.pos.pose.position.x = -0.14
self.pos.pose.position.y = -0.0434
self.pos.pose.position.z = 0.054
'''
self.g=Grasp()
self.g.pre_grasp_approach.direction.vector.z= 1
self.g.pre_grasp_approach.direction.header.frame_id = 'endeff'
self.g.pre_grasp_approach.min_distance = 0.04
self.g.pre_grasp_approach.desired_distance = 0.10
self.g.grasp_posture = self.make_gripper_posture(0)
self.g.post_grasp_retreat.direction.vector.z= -1
self.g.post_grasp_retreat.direction.header.frame_id = 'endeff'
self.g.post_grasp_retreat.min_distance = 0.04
self.g.post_grasp_retreat.desired_distance = 0.10
self.g.allowed_touch_objects = ["part"]
self.p=PlaceLocation()
self.p.place_pose.header.frame_id= 'world'
self.p.place_pose.pose.position.x= -0.13341
self.p.place_pose.pose.position.y= 0.12294
self.p.place_pose.pose.position.z= 0.099833
self.p.place_pose.pose.orientation.x= 0
self.p.place_pose.pose.orientation.y= 0
self.p.place_pose.pose.orientation.z= 0
self.p.place_pose.pose.orientation.w= 1
self.p.pre_place_approach.direction.vector.z= 1
self.p.pre_place_approach.direction.header.frame_id = 'endeff'
self.p.pre_place_approach.min_distance = 0.06
self.p.pre_place_approach.desired_distance = 0.12
self.p.post_place_posture= self.make_gripper_posture(-1.1158)
self.p.post_place_retreat.direction.vector.z= -1
self.p.post_place_retreat.direction.header.frame_id = 'endeff'
self.p.post_place_retreat.min_distance = 0.05
self.p.post_place_retreat.desired_distance = 0.06
self.p.allowed_touch_objects=["part"]
self._as = actionlib.SimpleActionServer("server_test", TwoIntsAction, execute_cb=self.execute_pick_place, auto_start = False)
self._as.start()
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moving_irb120_robot', anonymous=True)
robot = moveit_commander.RobotCommander()
#scene = moveit_commander.PlanningSceneInterface()
scene = moveit_commander.PlanningSceneInterface("base_link")
arm_group = moveit_commander.MoveGroupCommander("irb_120")
hand_group = moveit_commander.MoveGroupCommander("robotiq_85")
# Publish trajectory in RViz
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
grasp = Grasp()
place = PlaceLocation()
pick_and_place = PickPlaceInterface("irb_120", "robotiq_85")
# Inverse Kinematics (IK): move TCP to given position and orientation
def move_pose_arm(roll, pitch, yaw, x, y, z):
pose_goal = geometry_msgs.msg.Pose()
quat = quaternion_from_euler(roll, pitch, yaw)
pose_goal.orientation.x = quat[0]
pose_goal.orientation.y = quat[1]
pose_goal.orientation.z = quat[2]
pose_goal.orientation.w = quat[3]
pose_goal.position.x = x
pose_goal.position.y = y
pose_goal.position.z = z
arm_group.set_pose_target(pose_goal)
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)
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)
