def test_plan_to_waypoints_target(self): waypoints = [] waypoints.append(conversions.list_to_pose([0.71, 0.17, 0.34, 0, 0, 0, 1])) waypoints.append(conversions.list_to_pose([0.71, 0.15, 0.34, 0, 0, 0, 1])) waypoints.append(conversions.list_to_pose([0.69, 0.15, 0.34, 0, 0, 0, 1])) waypoints.append(conversions.list_to_pose([0.71, 0.17, 0.34, 0, 0, 0, 1])) (plan, f) = self.robot_commander.plan_to_waypoints_target(waypoints) self.assertIsInstance(plan, RobotTrajectory)
def move_to_pose(self, pose, vel=1.0, accel=1.0, wait=True): """ Move to pose :param pose: Array of positions and orientations [x, y, z, qx, qy, qz, qw] :param vel: Velocity (fraction of max) [0.0, 1.0] :param wait: to wait for completition of motion or not : :type pose: :type vel: :type accel: :type wait: bool : :return: success :rtype: bool """ if type(pose) is list: pose = list_to_pose(pose) self.moveit_arm_group.set_max_velocity_scaling_factor(vel) self.moveit_arm_group.set_max_acceleration_scaling_factor(accel) self.moveit_arm_group.set_pose_target(pose) success = self.moveit_arm_group.go(wait=wait) self.moveit_arm_group.stop() self.moveit_arm_group.clear_pose_targets() return success
def ik_joint_and_gripper_plan_execution(self, waypoint_list, gripper_last=True): # executes a trajectory based on ik computation with baxter interface API # if poses not as list of poses, will convert them into poses. # MUST BE PASSED A ZIPPED LIST OF JOINTS AND GRIPPERS # Example: #-- pose_action = [Pose1, Pose2, Pose3, ..., PoseN] # -- gripper_action = [0] * len(pose_action) # -- waypoint_list = zip(pose_action, gripper_action) # -- self.ik_planner.ik_joint_plan_execution(waypoint_list) for waypoint in waypoint_list: pose = waypoint[0] gripper_pos = waypoint[1] if type(pose) is list: pose = conversions.list_to_pose(pose) goal_joints = self._ik_request(pose) if not goal_joints: print "no ik solution found, terminate further execution and return False" return 1 else: if gripper_last: self._limb.move_to_joint_positions(goal_joints, timeout=5.0) self.gripper.command_position(gripper_pos) rospy.sleep(0.4) else: self.gripper.command_position(gripper_pos) rospy.sleep(0.8) self._limb.move_to_joint_positions(goal_joints, timeout=5.0) return 0
def any_plan(self, goal, **kwargs): assert len(goal) == 7, 'goal should be a list describing pose' pose_target = copy(goal) pose_target = conversions.list_to_pose(pose_target) self.group.set_planning_time(self._pt) plan = self.group.plan(pose_target) return plan
def move_to_pose_cartesian(self, pose, wait=True): #no support for vel, accel scaling for cartesian https://answers.ros.org/question/291887/moveit-acceleration-scaling-factor-not-working/ """ Move to pose following a cartesian path :param pose: geometry_msgs Pose :param wait: to wait for completition of motion or not : :type pose: :type wait: bool : :return: success :rtype: bool """ if type(pose) is list: pose = list_to_pose(pose) (plan, fraction) = self.moveit_arm_group.compute_cartesian_path( [pose], # waypoints to follow 0.005, # eef_step 0.0) # jump_threshold if fraction != 1.0: raise ValueError('Unable to plan entire path!') success = self.moveit_arm_group.execute(plan, wait=wait) self.moveit_arm_group.stop() self.moveit_arm_group.clear_pose_targets() return success
def goto_pose(self, pose, velocity=None, group_name=None, wait=True): if group_name: self.set_group(group_name) if not self.active_group: raise ValueError('No active Planning Group') if type(pose) is list: pose = list_to_pose(pose) self.active_group.set_max_velocity_scaling_factor( velocity if velocity else 0.5) self.active_group.set_pose_target(pose) self.result = None self.active_group.go(wait=False) if not wait: return True while not self.result: rospy.sleep(0.01) self.active_group.stop() self.active_group.clear_pose_targets() return self.result.result.error_code.val == 1
def goto_pose_cartesian(self, pose, velocity=1.0, group_name=None, wait=True): """ Move to pose following a cartesian trajectory. :param pose: Array position & orientation [x, y, z, qx, qy, qz, qw] :param velocity: Velocity (fraction of max) [0.0, 1.0] :param group_name: Move group (use current if None) :param wait: Wait for completion if True :return: Bool success """ if group_name: self.set_group(group_name) if not self.active_group: raise ValueError('No active Planning Group') if type(pose) is list: pose = list_to_pose(pose) self.active_group.set_max_velocity_scaling_factor(velocity) (plan, fraction) = self.active_group.compute_cartesian_path( [pose], # waypoints to follow 0.005, # eef_step 0.0) # jump_threshold if fraction != 1.0: raise ValueError('Unable to plan entire path!') success = self.active_group.execute(plan, wait=wait) self.active_group.stop() self.active_group.clear_pose_targets() return success
def move_pose_relative_world(self, dpose, velocity): if velocity is None: self.moveg.set_max_velocity_scaling_factor(self.velocity) else: self.moveg.set_max_velocity_scaling_factor(velocity) pose = pose_to_list(self.moveg.get_current_pose().pose) pose = np.append(pose[0:3], tr.euler_from_quaternion(pose[3:])) pose = np.add(pose, dpose) wp = [list_to_pose(pose)] self.moveg.set_start_state(self.robot.get_current_state()) (plan, fraction) = self.moveg.compute_cartesian_path(wp, eef_step=0.01, jump_threshold=0.0) if fraction < 1.0: self.last_error_msg = "No motion plan found." return False v = self.velocity if velocity is None else velocity plan = self.moveg.retime_trajectory(self.robot.get_current_state(), plan, v) try: self.moveg.execute(plan, wait=True) self.moveg.stop() except MoveItCommanderException as e: self.last_error_msg = str(e) return False return True
def cartesian_plan(self, waypoint_list, interpolation=0.01, jump_threshold=0.0): waypoints = [self.group.get_current_pose().pose] if type(waypoint_list[0]) is list: waypoints = waypoints + [ conversions.list_to_pose(copy(waypoint)) for waypoint in waypoint_list ] else: waypoints = waypoints + conversions.list_to_pose( copy(waypoint_list)) self.group.set_planning_time(self._pt) plan, fraction = self.group.compute_cartesian_path( waypoints, interpolation, jump_threshold) rospy.sleep(self._pt) return plan, fraction
def affine2pose(T): """Convert affine transformation to Pose. Example: >>> T = np.array([[-1,0,0,1],[0,1,0,2],[0,0,-1,3],[0,0,0,1]]) >>> conversions.pose_to_list(affine2pose(T)) [1.0, 2.0, 3.0, 0.0, 1.0, 0.0, 0.0] """ q = mat2q(T[0:3][:, 0:3]) t = T[np.ix_([0, 1, 2], [3])] return conversions.list_to_pose(np.append(t, q))
def change_end_pose(group, input_pose): end_goal = pose_to_list(group.get_current_pose().pose) for i, ele in enumerate(input_pose): if ele != None: end_goal[i] = ele # print "\nOUT: ", input_pose, end_goal group.set_pose_target(list_to_pose(end_goal)) if group.go(wait=True): # if group.plan(): print "\nSolution found for ", input_pose print "\nGroup moved to ", end_goal else: print "\nNo solution found for ", input_pose
def add_pose(pose_a, pose_b): """ Add two poses """ position_a, orientation_a = pose_to_lists(pose_a, 'euler') position_b, orientation_b = pose_to_lists(pose_b, 'euler') position_c = [a + b for a, b in zip(position_a, position_b)] orientation_c = [a + b for a, b in zip(orientation_a, orientation_b)] pose_c = list_to_pose(position_c + orientation_c) return pose_c
def change_end_pose(group, input_pose): pose_goal = geometry_msgs.msg.Pose() end_goal = pose_to_list(pose_goal) for i, ele in enumerate(input_pose): if ele != None: end_goal[i] = ele group.set_pose_target(list_to_pose(end_goal)) if group.go(wait=True): print "Solution found for ", input_pose print "Group moved to ", end_goal else: print "No solution found for", input_pose group.stop() group.clear_pose_targets()
def get_TF_to_pose(a,b): end_flag = 0 listener = tf.TransformListener() while end_flag ==0: try: (trans,rot) = listener.lookupTransform(a,b, rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue end_flag = 1 euler = euler_from_quaternion([rot[0],rot[1],rot[2],rot[3]]) pose_target_euler = [trans[0],trans[1],trans[2],euler[0]-1.57,euler[1],euler[2]+3.14] pose_target = list_to_pose(pose_target_euler) return pose_target
def add_box_object(self, id_name, pose, size = (1, 1, 1), rotation = (0, 0, 0), frame='/world'): ''' Adds the particular BOX TYPE objects to the moveit planning scene :param id_name: unique id that object should be labeled with :param pose: pose of the object :param size: size of the object :param rotation: rotation offset in r, p, y :param frame: frame in which the object pose is passed :type id_name: string :type pose: list of double of length 7 (x, y, z, q_x, q_y, q_z, q_w) :type size: tuple of length 3 :type frame: string ''' assert type(size) is tuple, 'size should be tuple' assert len(size)==3, 'size should be of length 3' assert not id_name in self.scene_objects, \ 'Object with the same name already exists!' self.scene_objects.append(id_name) object_pose = PoseStamped() object_pose.header.frame_id = frame if type(pose) is list: pose = list_to_pose(pose) (r, p, y) = rotation q_object_rotation = quaternion_from_euler(math.radians(r), math.radians(p), math.radians(y)) q_object = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w] q_object_final = quaternion_multiply(q_object, q_object_rotation) x = pose.position.x y = pose.position.y z = pose.position.z object_pose.pose = Pose(Point(x, y, z), Quaternion(*q_object_final)) #Add object description in scene self.clear_octomap() #need to sleep before adding part!! https://answers.ros.org/question/209030/moveit-planningsceneinterface-addbox-not-showing-in-rviz/ self.planning_scene_interface.add_box(id_name, object_pose, size) self.wait_for_planning_scene_object(id_name)
def goto_pose(self, pose, velocity=1.0, group_name=None, wait=True): """ Move to pose :param pose: Array position & orientation [x, y, z, qx, qy, qz, qw] :param velocity: Velocity (fraction of max) [0.0, 1.0] :param group_name: Move group (use current if None) :param wait: Wait for completion if True :return: Bool success """ if group_name: self.set_group(group_name) if not self.active_group: raise ValueError('No active Planning Group') if type(pose) is list: pose = list_to_pose(pose) self.active_group.set_max_velocity_scaling_factor(velocity) self.active_group.set_pose_target(pose) success = self.active_group.go(wait=wait) self.active_group.stop() self.active_group.clear_pose_targets() return success
def move_pose_relative(self, dpose, velocity): if velocity is None: self.moveg.set_max_velocity_scaling_factor(self.velocity) else: self.moveg.set_max_velocity_scaling_factor(velocity) pose = pose_to_list(self.moveg.get_current_pose().pose) t_xform = np.dot(tr.translation_matrix(pose[0:3]), tr.quaternion_matrix(pose[3:])) s_xform = np.dot(tr.translation_matrix(dpose[0:3]), tr.euler_matrix(*dpose[3:])) xform = np.dot(t_xform, s_xform) pose = tr.translation_from_matrix(xform).tolist() + list( tr.euler_from_matrix(xform)) wp = [list_to_pose(pose)] # waypoints self.moveg.set_start_state(self.robot.get_current_state()) (plan, fraction) = self.moveg.compute_cartesian_path(wp, eef_step=0.01, jump_threshold=0.0) if fraction < 1.0: self.last_error_msg = "No motion plan found." return False v = self.velocity if velocity is None else velocity plan = self.moveg.retime_trajectory(self.robot.get_current_state(), plan, v) try: self.moveg.execute(plan, wait=True) self.moveg.stop() except MoveItCommanderException as e: self.last_error_msg = str(e) return False return True
def add_world_object(self, id_name, pose, size, frame='/base'): ''' Adds the particular object to the moveit planning scene :param id_name: unique id that object should be labeled with :param pose: pose of the object :param size: size of the object :param frame: frame in which the object pose is passed :type id_name: string :type pose: list of double of length 7 (x,y,z, q_x, q_y, q_z, q_w) :type size: tuple of length 3 :type frame: string ''' assert type(size) is tuple, 'size should be tuple' assert len(size) == 3, 'size should be of length 3' pose = conversions.list_to_pose(pose) pose_stamped = PoseStamped() pose_stamped.header.frame_id = frame pose_stamped.pose = pose self.scene.add_box(id_name, pose_stamped, size) rospy.sleep(1.0) self.scene.add_box(id_name, pose_stamped, size)
def goto_pose_cartesian(self, pose, velocity=None, group_name=None, wait=True): if group_name: self.set_group(group_name) if not self.active_group: raise ValueError('No active Planning Group') if type(pose) is list: pose = list_to_pose(pose) (plan, fraction) = self.active_group.compute_cartesian_path( [pose], # waypoints to follow 0.005, # eef_step 0.0) # jump_threshold plan = self.active_group.retime_trajectory( self.robot.get_current_state(), plan, velocity if velocity else 0.5) if fraction != 1.0: raise ValueError('Unable to plan entire path!') return False self.result = None self.active_group.execute(plan, wait=False) if not wait: return True while not self.result: rospy.sleep(0.01) self.active_group.stop() self.active_group.clear_pose_targets() return self.result.result.error_code.val == 1
def attach_arm_object(self, link_name, id_name, pose, size): ''' Attaches the specified object to the robot :param link_name: name of the link to which the bject should be attached :param id_name: unique id associated with the object :param pose: pose of the object :parma size: size of the object :type link_name: string :type id_name: string :type pose: list of double of length 7 (x,y,z, q_x, q_y, q_z, q_w) :type size: tuple of length 3 ''' assert type(size) is tuple, 'size should be tuple' assert len(size) == 3, 'size should be of length 3' pose = conversions.list_to_pose(pose) pose_stamped = PoseStamped() pose_stamped.header.frame_id = link_name pose_stamped.pose = pose self.scene.attach_box(link_name, id_name, pose_stamped, size) rospy.sleep(1.0) self.scene.attach_box(link_name, id_name, pose_stamped, size)
# ar마커 변수 선언 ar_x = 0 ar_y = 0 ar_z = 0 #move_Joint(1.57,-2.27,1.93,-1.19,-1.57,0) #down pose move_Joint(0, 0, 0, 0, 0, 0) #down pose rospy.sleep(3) husky_tr, husky_rot = get_TF('odom', 'base_link') husky_x = husky_tr[0] husky_y = husky_tr[1] # 사용자입력 : 목표 eef 위치 지정 (x,y,z,r,p,yaw) goal_eef = [0, 0, 0, 0, 0, 0, 1] goal_eef_quat = list_to_pose(goal_eef) # 현재 eef pose 받아오기 eef_pose = Pose() eef_tr, eef_rot = get_TF('base_link', 'rh_p12_rn_base') #vector_x = ar_tr[0] - eef_tr[0] #vector_y = ar_tr[1] - eef_tr[1] #angle_to_goal = atan2(vector_y,vector_x) #distance_to_goal_eef = sqrt(vector_x**2 + vector_y**2) # ar 마커 및 eef pose data 선언. ar_pose = Pose() eef_pose = Pose() ar_tr, ar_rot = get_TF('base_link', 'ar_marker_0')
def test_get_ik(self): pose = PoseStamped() pose.header.stamp = rospy.get_rostime() pose.pose = conversions.list_to_pose([0.71, 0.17, 0.34, 0, 0, 0, 1]) joint_state_from_ik = self.robot_commander.get_ik(pose) self.assertIsInstance(joint_state_from_ik, JointState)
def get_pose(self): print("Current tool pose is: ", self.rob.getl()) return list_to_pose(self.rob.getl())
from moveit_commander.conversions import list_to_pose import copy approach_height = 0.08 approach_offset = 0.05 positions = {} positions["back"] = list_to_pose([0, 0, 0, 0, 0, 0]) positions["back_up"] = list_to_pose([0, 0, approach_height, 0, 0, 0]) positions["back_out"] = list_to_pose([-approach_offset, 0, 0, 0, 0, 0]) positions["back_out_up"] = list_to_pose( [-approach_offset, 0, approach_height, 0, 0, 0]) positions["left"] = list_to_pose([0, 0, 0, 0, 0, 1.5707]) positions["left_up"] = list_to_pose([0, 0, approach_height, 0, 0, 1.5707]) positions["left_out"] = list_to_pose([0, -approach_offset, 0, 0, 0, 1.5707]) positions["left_out_up"] = list_to_pose( [0, -approach_offset, approach_height, 0, 0, 1.5707]) positions["right"] = list_to_pose([0, 0, 0, 0, 0, -1.5707]) positions["right_up"] = list_to_pose([0, 0, approach_height, 0, 0, -1.5707]) positions["right_out"] = list_to_pose([0, approach_offset, 0, 0, 0, -1.5707]) positions["right_out_up"] = list_to_pose( [0, approach_offset, approach_height, 0, 0, -1.5707]) positions["back_turned"] = list_to_pose([0, 0, 0, 1.5707, 0, 0]) positions["back_turned_up"] = list_to_pose( [0, 0, approach_height, 1.5707, 0, 0]) positions["back_turned_out"] = list_to_pose( [-approach_offset, 0, 0, 1.5707, 0, 0]) positions["back_turned_out_up"] = list_to_pose( [-approach_offset, 0, approach_height, 1.5707, 0, 0])
def add_dynamic_obj(self, ref_frame, obj_name, pos, ori, size, touch_links=None): """ Add object to the ref_frame, the object will move with the ref_frame. Only box is supported for now. Args: ref_frame (str): which link are you adding object to. obj_name (str): object name. pos (list): position of the object with respect to the ref_frame. ori (list): orientation of the object with respect to the ref_frame. size (float or list or tuple): size can be a float, which means the edge length of a cube. size can also be a list or tuple of length 3, the it specifies the 3 edge lengths of the cuboid. Returns: bool: if the object is successfully added. """ if not isinstance(pos, list): raise TypeError('pos should be a list') if not isinstance(ori, list): raise TypeError('ori should be a list') pose = pos + ori pose = conversions.list_to_pose(pose) pose_stamped = PoseStamped() pose_stamped.header.frame_id = ref_frame pose_stamped.pose = pose if isinstance(size, float): size = (size, size, size) elif isinstance(size, list) or isinstance(size, tuple): if len(size) != 3: raise ValueError('If size is a list or tuple, its length' ' should be 3 for a box') else: raise TypeError('size should be a float number, a 3-element list ' 'or a 3-element tuple for a box') if isinstance(size, list): size = tuple(size) if touch_links is None: self.scene.attach_box(ref_frame, obj_name, pose_stamped, size) else: # moveit ignores collisions between box and links in touch_links self.scene.attach_box(ref_frame, obj_name, pose_stamped, size, touch_links=touch_links) obj_dict, obj_adict = self.get_objects() success = False if obj_name in obj_adict.keys(): success = True return success
def main(): try: pnp = PickNPlaceTutorial() pnp.jmove_to_joint_goal([1.57, -2.27, 1.93, -1.19, -1.57, 0]) #home pose table = geometry_msgs.msg.PoseStamped() table.header.frame_id = "odom" table.pose.position.x = 0.75 table.pose.position.y = 0.1 table.pose.position.z = 0.7 table.pose.orientation.w = 1 pnp.add_box(name='table', pose_stamp=table, size=(0.1, 0.1, 0.3)) i = 0 while (i < 10): test_pose = list_to_pose([0.75, 0.1, 0.7, 0, 9 * pi / 180 * i, 0]) table.pose.position.x = test_pose.position.x table.pose.position.y = test_pose.position.y table.pose.position.z = test_pose.position.z table.pose.orientation.x = test_pose.orientation.x table.pose.orientation.y = test_pose.orientation.y table.pose.orientation.z = test_pose.orientation.z table.pose.orientation.w = test_pose.orientation.w table.pose.position.x = table.pose.position.x - 0.1 table.pose.position.y = table.pose.position.y - 0.1 table.pose.position.z = table.pose.position.z - 0.1 pnp.add_box(name='table', pose_stamp=table, size=(0.1, 0.1, 0.1)) rospy.sleep(0.1) print(table.pose) pnp.jmove_to_pose_goal(table.pose) rospy.sleep(0.1) i += 1 i = 0 while (i < 10): test_pose = list_to_pose( [0.75, 0.1, 0.7, 0, pi / 2 - i * 9 * pi / 180, 0]) table.pose.position.x = test_pose.position.x table.pose.position.y = test_pose.position.y table.pose.position.z = test_pose.position.z table.pose.orientation.x = test_pose.orientation.x table.pose.orientation.y = test_pose.orientation.y table.pose.orientation.z = test_pose.orientation.z table.pose.orientation.w = test_pose.orientation.w table.pose.position.x = table.pose.position.x - 0.1 table.pose.position.y = table.pose.position.y - 0.1 table.pose.position.z = table.pose.position.z - 0.1 pnp.add_box(name='table', pose_stamp=table, size=(0.1, 0.1, 0.1)) rospy.sleep(0.1) print(table.pose) pnp.jmove_to_pose_goal(table.pose) rospy.sleep(0.1) i += 1 i = 0 while (i < 5): test_pose = list_to_pose([0.75 + 0.01 * i, 0.1, 0.7, 0, 0, 0]) table.pose.position.x = test_pose.position.x table.pose.position.y = test_pose.position.y table.pose.position.z = test_pose.position.z table.pose.orientation.x = test_pose.orientation.x table.pose.orientation.y = test_pose.orientation.y table.pose.orientation.z = test_pose.orientation.z table.pose.orientation.w = test_pose.orientation.w table.pose.position.x = table.pose.position.x - 0.1 table.pose.position.y = table.pose.position.y - 0.1 table.pose.position.z = table.pose.position.z - 0.1 pnp.add_box(name='table', pose_stamp=table, size=(0.1, 0.1, 0.1)) rospy.sleep(0.1) print(table.pose) pnp.jmove_to_pose_goal(table.pose) rospy.sleep(0.1) i += 1 i = 0 while (i < 5): test_pose = list_to_pose([0.85 - 0.01 * i, 0.1, 0.7, 0, 0, 0]) table.pose.position.x = test_pose.position.x table.pose.position.y = test_pose.position.y table.pose.position.z = test_pose.position.z table.pose.orientation.x = test_pose.orientation.x table.pose.orientation.y = test_pose.orientation.y table.pose.orientation.z = test_pose.orientation.z table.pose.orientation.w = test_pose.orientation.w table.pose.position.x = table.pose.position.x - 0.1 table.pose.position.y = table.pose.position.y - 0.1 table.pose.position.z = table.pose.position.z - 0.1 pnp.add_box(name='table', pose_stamp=table, size=(0.1, 0.1, 0.1)) rospy.sleep(0.1) print(table.pose) pnp.jmove_to_pose_goal(table.pose) rospy.sleep(0.1) i += 1 i = 0 while (i < 5): test_pose = list_to_pose([0.75, 0.1 + 0.01 * i, 0.7, 0, 0, 0]) table.pose.position.x = test_pose.position.x table.pose.position.y = test_pose.position.y table.pose.position.z = test_pose.position.z table.pose.orientation.x = test_pose.orientation.x table.pose.orientation.y = test_pose.orientation.y table.pose.orientation.z = test_pose.orientation.z table.pose.orientation.w = test_pose.orientation.w table.pose.position.x = table.pose.position.x - 0.1 table.pose.position.y = table.pose.position.y - 0.1 table.pose.position.z = table.pose.position.z - 0.1 pnp.add_box(name='table', pose_stamp=table, size=(0.1, 0.1, 0.1)) rospy.sleep(0.1) print(table.pose) pnp.jmove_to_pose_goal(table.pose) rospy.sleep(0.1) i += 1 i = 0 while (i < 10): test_pose = list_to_pose([0.75, 0.15 - 0.01 * i, 0.7, 0, 0, 0]) table.pose.position.x = test_pose.position.x table.pose.position.y = test_pose.position.y table.pose.position.z = test_pose.position.z table.pose.orientation.x = test_pose.orientation.x table.pose.orientation.y = test_pose.orientation.y table.pose.orientation.z = test_pose.orientation.z table.pose.orientation.w = test_pose.orientation.w table.pose.position.x = table.pose.position.x - 0.1 table.pose.position.y = table.pose.position.y - 0.1 table.pose.position.z = table.pose.position.z - 0.1 pnp.add_box(name='table', pose_stamp=table, size=(0.1, 0.1, 0.1)) rospy.sleep(0.1) print(table.pose) pnp.jmove_to_pose_goal(table.pose) rospy.sleep(0.1) i += 1 i = 0 while (i < 5): test_pose = list_to_pose([0.75, 0.05 + 0.01 * i, 0.7, 0, 0, 0]) table.pose.position.x = test_pose.position.x table.pose.position.y = test_pose.position.y table.pose.position.z = test_pose.position.z table.pose.orientation.x = test_pose.orientation.x table.pose.orientation.y = test_pose.orientation.y table.pose.orientation.z = test_pose.orientation.z table.pose.orientation.w = test_pose.orientation.w table.pose.position.x = table.pose.position.x - 0.1 table.pose.position.y = table.pose.position.y - 0.1 table.pose.position.z = table.pose.position.z - 0.1 pnp.add_box(name='table', pose_stamp=table, size=(0.1, 0.1, 0.1)) rospy.sleep(0.1) print(table.pose) pnp.jmove_to_pose_goal(table.pose) rospy.sleep(0.1) i += 1 pnp.jmove_to_joint_goal([1.57, -2.27, 1.93, -1.19, -1.57, 0]) #home pose # #pnp.jmove_to_pose_goal(list_to_pose([0.35, -0.10, 0.48, 0, -pi, -pi/2])) #pnp.tmove_to_pose_goal(list_to_pose([0.35, -0.10, 0.18, 0, -pi, -pi/2])) #pnp.tmove_to_pose_goal(list_to_pose([0.35, -0.10, 0.28, 0, -pi, -pi/2])) # # #pnp.jmove_to_pose_goal(list_to_pose([0.65, -0.10, 0.48, 0, -pi, -pi/2])) #pnp.tmove_to_pose_goal(list_to_pose([0.65, -0.10, 0.18, 0, -pi, -pi/2])) #pnp.tmove_to_pose_goal(list_to_pose([0.65, -0.10, 0.28, 0, -pi, -pi/2])) # # #pnp.jmove_to_pose_goal(list_to_pose([0.35, 0.10, 0.48, 0, -pi, -pi/2])) #pnp.tmove_to_pose_goal(list_to_pose([0.35, 0.10, 0.18, 0, -pi, -pi/2])) #pnp.tmove_to_pose_goal(list_to_pose([0.35, 0.10, 0.28, 0, -pi, -pi/2])) # # #pnp.jmove_to_pose_goal(list_to_pose([0.65, 0.10, 0.48, 0, -pi, -pi/2])) #pnp.tmove_to_pose_goal(list_to_pose([0.65, 0.10, 0.18, 0, -pi, -pi/2])) #pnp.tmove_to_pose_goal(list_to_pose([0.65, 0.10, 0.28, 0, -pi, -pi/2])) # #pnp.jmove_to_joint_goal([0, 0, -pi/2, 0, -pi/2, 0]) except rospy.ROSInterruptException: return except KeyboardInterrupt: return else: print("============ Python pick place demo complete!")
def main(): try: pnp = PickNPlaceTutorial() raw_input() table = geometry_msgs.msg.PoseStamped() table.header.frame_id = pnp.robot_group.get_planning_frame() table.pose.position.x =0.5 table.pose.position.y =0 table.pose.position.z =0.05 table.pose.orientation.w = 1.0 pnp.add_box(name='table', pose_stamp=table, size=(0.5, 0.5, 0.1)) wall = geometry_msgs.msg.PoseStamped() wall.header.frame_id = pnp.robot_group.get_planning_frame() wall.pose.position.x =0.5 wall.pose.position.y =0 wall.pose.position.z =0.2 wall.pose.orientation.w = 1.0 pnp.add_box(name='wall', pose_stamp=wall, size=(0.01, 0.5, 0.25)) cube_0 = geometry_msgs.msg.PoseStamped() cube_0.header.frame_id = pnp.robot_group.get_planning_frame() cube_0.pose.position.x =0.35 cube_0.pose.position.y =-0.10 cube_0.pose.position.z =0.13 cube_0.pose.orientation.w = 1.0 pnp.add_box(name='cube_0', pose_stamp=cube_0) cube_1 = geometry_msgs.msg.PoseStamped() cube_1.header.frame_id = pnp.robot_group.get_planning_frame() cube_1.pose.position.x =0.35 cube_1.pose.position.y =0.10 cube_1.pose.position.z =0.13 cube_1.pose.orientation.w = 1.0 pnp.add_box(name='cube_1', pose_stamp=cube_1) rospy.sleep(0.5) pnp.jmove_to_joint_goal([0, 0, -pi/2, 0, -pi/2, 0]) pnp.jmove_to_pose_goal(list_to_pose([0.35, -0.10, 0.48, 0, -pi, -pi/2])) pnp.tmove_to_pose_goal(list_to_pose([0.35, -0.10, 0.18, 0, -pi, -pi/2])) pnp.hold_hand('cube_0') pnp.tmove_to_pose_goal(list_to_pose([0.35, -0.10, 0.28, 0, -pi, -pi/2])) pnp.jmove_to_pose_goal(list_to_pose([0.65, -0.10, 0.48, 0, -pi, -pi/2])) pnp.tmove_to_pose_goal(list_to_pose([0.65, -0.10, 0.18, 0, -pi, -pi/2])) pnp.release_hand('cube_0') pnp.tmove_to_pose_goal(list_to_pose([0.65, -0.10, 0.28, 0, -pi, -pi/2])) pnp.jmove_to_pose_goal(list_to_pose([0.35, 0.10, 0.48, 0, -pi, -pi/2])) pnp.tmove_to_pose_goal(list_to_pose([0.35, 0.10, 0.18, 0, -pi, -pi/2])) pnp.hold_hand('cube_1') pnp.tmove_to_pose_goal(list_to_pose([0.35, 0.10, 0.28, 0, -pi, -pi/2])) pnp.jmove_to_pose_goal(list_to_pose([0.65, 0.10, 0.48, 0, -pi, -pi/2])) pnp.tmove_to_pose_goal(list_to_pose([0.65, 0.10, 0.18, 0, -pi, -pi/2])) pnp.release_hand('cube_1') pnp.tmove_to_pose_goal(list_to_pose([0.65, 0.10, 0.28, 0, -pi, -pi/2])) pnp.jmove_to_joint_goal([0, 0, -pi/2, 0, -pi/2, 0]) except rospy.ROSInterruptException: return except KeyboardInterrupt: return else: print("============ Python pick place demo complete!")
def add_static_obj(self, obj_name, pos, ori, size=None, obj_type='box', ref_frame='/base_link', normal=None): """ Add static object to the planning scene. Args: obj_name (str): object name. pos (list): position. ori (list): orientation. It can be quaternion ([qx, qy, qz, qw]) or euler angles ([roll, pitch, yaw]). size (float or list or tuple): size of the object. if the object is a plane, size should be None if the object is a box, size can be a float (which means the edge length of a cube.) size can also be a list or tuple of length 3, that specifies the 3 edge lengths of the cuboid if the object is a sphere, size is a float, (which means the radius). obj_type (str): one of [`sphere`, `box`, `plane`]. ref_frame (str): reference frame on which the pos and ori are specified. normal (list or tuple): only used if the object is a plane. It means the normal direction of the plane. Returns: bool: if the object is successfully added. """ if not isinstance(pos, list): raise TypeError('pos should be a list') if not isinstance(ori, list): raise TypeError('ori should be a list') if obj_type not in ['sphere', 'box', 'plane']: raise ValueError('Unsupported object type. Only [sphere],' ' [box], [plane] are supported for now.') pose = pos + ori pose = conversions.list_to_pose(pose) pose_stamped = PoseStamped() pose_stamped.header.frame_id = ref_frame pose_stamped.pose = pose if obj_type == 'plane': if size is not None: raise ValueError('No size needed for plane') if normal is None: normal = (0, 0, 1) if len(normal) != 3: raise ValueError('Length of the normal should be 3') if isinstance(normal, list): normal = tuple(normal) self.scene.add_plane(obj_name, pose_stamped, normal) elif obj_type == 'box': if isinstance(size, float): size = (size, size, size) elif isinstance(size, list) or isinstance(size, tuple): if len(size) != 3: raise ValueError('If size is a list or tuple, its length' ' should be 3 for a box') else: raise TypeError('size should be a float number, ' 'a 3-element list ' 'or a 3-element tuple for a box') if isinstance(size, list): size = tuple(size) self.scene.add_box(obj_name, pose_stamped, size) else: if not isinstance(size, float): raise ValueError('Size should a float number for sphere') self.scene.add_sphere(obj_name, pose_stamped, radius=size) obj_dict, obj_adict = self.get_objects() success = False if obj_name in obj_dict.keys(): success = True return success