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 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)