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 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 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 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
n.close_gripper(TOOL_GRIPPER_3_ID, 200)
print("[GRIPPER 3 CLOSED]")
#Check pickup planning result
pevent("Planner returned: " +
get_moveit_error_code(pick_result.error_code.val))
if pick_result.error_code.val == 1:
pevent("Grasp successful!")
else:
pevent("Grasp failed. Aborting")
#Fill place msg
l = PlaceLocation()
l.id = "niryo_place"
lp = PoseStamped()
lp.header.frame_id = "base_link"
lp.place_pose.position.x = 15
lp.place_pose.position.y = 10
lp.place_pose.position.z = 0
lp.place_pose.orientation.x = 0
lp.place_pose.orientation.y = 0
lp.place_pose.orientation.z = 0
lp.place_pose.orientation.w = 1
l.place_pose = lp
l.post_place_retreat.direction.frame_id = "hand_link"
l.post_place_retreat.direction.vector.x = 0
