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: