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 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):
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.pose.position.x = self.place_x
p.pose.position.y = self.place_y
p.pose.position.z = self.box_z
# 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 = self.robot.get_planning_frame(
)
g.pre_place_approach.direction.header.frame_id = "l_wrist_roll_link"
g.pre_place_approach.min_distance = 0.0
g.pre_place_approach.desired_distance = 0.0
g.post_place_retreat.min_distance = 0.0
g.post_place_retreat.desired_distance = 0.05
g.post_place_posture = self._open_gripper(g.post_place_posture)
self.group = self.robot.get_group("left_arm")
self.group.set_support_surface_name("table")
# group.set_path_constraints(constr)
self.group.set_planner_id("RRTConnectkConfigDefault")
self.group.place("box", g)
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 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 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 = 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, 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 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 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
#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
l.post_place_retreat.direction.vector.y = 0
l.post_place_retreat.direction.vector.z = 1
l.post_place_retreat.min_distance = 0.05
l.post_place_retreat.desired_distance = 0.1
#l.allowed_touch_objects = ["<octomap>", "obj"]
l.post_place_posture.header.frame_id = "hand_link"
pos = JointTrajectoryPoint()
pos.positions.append(0)
l.post_place_posture.points.append(pos)
if mark_pose:
