def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('get_camera_pose_image_auto') # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('arm') # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.001) arm.set_goal_orientation_tolerance(0.001) arm.set_max_velocity_scaling_factor(0.8) arm.set_max_acceleration_scaling_factor(0.5) arm.set_planning_time(0.5) # 规划时间限制为2秒 arm.allow_replanning(False) # 当运动规划失败后,是否允许重新规划 # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) # 获取终端link的名称 eef_link = arm.get_end_effector_link() # 开始图像接收 image_receiver = ImageReceiver() self.group = arm self.eef_link = eef_link self.reference_frame = reference_frame self.moveit_commander = moveit_commander self.listener = tf.TransformListener() self.save_cnt = 0 print "\n\n[INFO] Pose image saver started."
def __init__(self, pose): self.check_collision = False # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_obstacles_demo') # 初始化场景对象 scene = PlanningSceneInterface() # 创建一个发布场景变化信息的发布者 self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) # 创建一个存储物体颜色的字典对象 self.colors = dict() # 等待场景准备就绪 rospy.sleep(1) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('arm') # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.05) # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) # 设置每次运动规划的时间限制:5s arm.set_planning_time(5) # 将场景中的颜色设置发布 self.sendColors() rospy.sleep(5) joint_positions = pose arm.set_joint_value_target(joint_positions) # 控制机械臂完成运动 if arm.go(): pass else: self.check_collision = True rospy.sleep(1) # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_cartesian_demo', anonymous=True) # 是否需要使用笛卡尔空间的运动规划 cartesian = rospy.get_param('~cartesian', True) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('arm') # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 # arm.set_goal_position_tolerance(0.00001) # arm.set_goal_orientation_tolerance(0.00001) arm.set_max_velocity_scaling_factor(0.5) arm.set_max_acceleration_scaling_factor(1.0) arm.set_planning_time(0.5) # 规划时间限制为2秒 arm.allow_replanning(True) # 当运动规划失败后,是否允许重新规划 # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) # 获取终端link的名称 eef_link = arm.get_end_effector_link() self.group = arm self.eef_link = eef_link self.reference_frame = reference_frame self.moveit_commander = moveit_commander self.listener = tf.TransformListener()
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('start_pos') # Use the planning scene object to add or remove objects scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=5) # Create a dictionary to hold object colors self.colors = dict() # Initialize the move group for the right arm right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.05) right_arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the right arm reference frame right_arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 10 seconds per planning attempt right_arm.set_planning_time(10) # Start the arm in the "resting" pose stored in the SRDF file right_arm.set_named_target('right_start') right_arm.go() # Open the gripper to the neutral position right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) #robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(5) self.robot_arm.set_num_planning_attempts(5) self.pose_goal = Pose() rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) def move_code(self): self.robot_arm.set_named_target("home") #go to goal state. self.robot_arm.go(wait=True) print("====== move plan go to home 1 ======") rospy.sleep(0.5) # print("====== move plan go to up ======") self.robot_arm.set_named_target("up") #go to goal state. self.robot_arm.go(wait=True) print("====== move plan go to up ======") rospy.sleep(0.5) self.robot_arm.set_named_target("home") #go to goal state. self.robot_arm.go(wait=True) print("====== move plan go to home 1 ======") rospy.sleep(0.5) # robot_arm.set_named_target("up") # robot_arm.go(wait=True) robot_state = self.robot_arm.get_current_pose() robot_angle = self.robot_arm.get_current_joint_values() print(robot_state) def move_TF(self): self.ee_TF_list = [-2.046, 1.121, 0.575, 0.453, 0.561, -0.435, 0.538] self.pose_goal.position.x = self.ee_TF_list[0] self.pose_goal.position.y = self.ee_TF_list[1] self.pose_goal.position.z = self.ee_TF_list[2] self.pose_goal.orientation.x = self.ee_TF_list[3] self.pose_goal.orientation.y = self.ee_TF_list[4] self.pose_goal.orientation.z = self.ee_TF_list[5] self.pose_goal.orientation.w = self.ee_TF_list[6] self.robot_arm.set_pose_target(self.pose_goal) self.robot_arm.go(True)
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_cartesian_demo', anonymous=True) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('arm') robot = moveit_commander.RobotCommander() # 当运动规划失败后,允许重新规划 arm.allow_replanning(False) # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.001) arm.set_goal_orientation_tolerance(0.01) arm.set_max_velocity_scaling_factor(0.5) arm.set_max_acceleration_scaling_factor(0.5) arm.set_planning_time(0.08) # 规划时间限制为2秒 # arm.set_num_planning_attempts(1) # 规划1次 # 获取终端link的名称 eef_link = arm.get_end_effector_link() scene = moveit_commander.PlanningSceneInterface() scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) print "[INFO] Current pose:\n", arm.get_current_pose().pose self.scene = scene self.scene_pub = scene_pub self.colors = dict() self.group = arm self.robot = robot self.eef_link = eef_link self.reference_frame = reference_frame self.moveit_commander = moveit_commander self.broadcaster = tf.TransformBroadcaster() self.listener = tf.TransformListener() self.gripper_len = 0.095 # 手爪实际长度0.165m, 虚拟夹爪深度0.075m 0.16-0.065=0.095m self.approach_distance = 0.06 self.back_distance = 0.05 # sub and pub point cloud self.point_cloud = None self.update_cloud_flag = False rospy.Subscriber('/camera/depth/color/points', PointCloud2, self.callback_pointcloud) thread.start_new_thread(self.publish_pointcloud, ())
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.Pose_goal = Pose() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) # robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(5) self.robot_arm.set_num_planning_attempts(5) rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) def move_code(self): # self.robot_arm.set_named_target("up") #go to goal state. # self.robot_arm.go() # print("====== move plan go to home 1 ======") # rospy.sleep(2) # print("====== move plan go to up ======") # self.robot_arm.set_named_target("zeros") #go to goal state. # self.robot_arm.go(wait=True) # print("====== move plan go to zeros ======") # rospy.sleep(1) # robot_arm.set_named_target("up") # robot_arm.go(wait=True) self.robot_arm.set_pose_target(self.Pose_goal) # go to goal state. self.robot_arm.go(True) print("====== move plan go to Pose_goal ======") #rospy.sleep(2) robot_state = self.robot_arm.get_current_pose(); robot_angle = self.robot_arm.get_current_joint_values(); print(robot_state) def callback(self, data): rospy.loginfo(rospy.get_caller_id() + "I heard %s", data) self.Pose_goal = data self.move_code() def listener(self): rospy.Subscriber("chatter", Pose, self.callback)
def arm_pose(): arm = MoveGroupCommander('arm') arm.allow_replanning(True) end_effector_link = arm.get_end_effector_link() arm.set_goal_position_tolerance(0.03) arm.set_goal_orientation_tolerance(0.025) arm.allow_replanning(True) reference_frame = 'base_footprint' arm.set_pose_reference_frame(reference_frame) arm.set_planning_time(5) curr_pose = arm.get_current_pose(end_effector_link).pose.position return curr_pose.x, curr_pose.y, curr_pose.z
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_cartesian_demo', anonymous=True) # 是否需要使用笛卡尔空间的运动规划 cartesian = rospy.get_param('~cartesian', True) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('arm') # 当运动规划失败后,允许重新规划 arm.allow_replanning(False) # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.001) arm.set_goal_orientation_tolerance(0.01) arm.set_max_velocity_scaling_factor(0.8) arm.set_max_acceleration_scaling_factor(0.5) arm.set_planning_time(1) # 规划时间限制为2秒 arm.set_num_planning_attempts(2) # 规划两次 # 获取终端link的名称 eef_link = arm.get_end_effector_link() scene = moveit_commander.PlanningSceneInterface() print arm.get_current_pose(eef_link).pose sub = rospy.Subscriber('/detect_grasps_yolo/juggle_rects', Float64MultiArray, self.callback) self.juggle_rects = Float64MultiArray() self.box_name = '' self.scene = scene self.group = arm self.eef_link = eef_link self.reference_frame = reference_frame self.moveit_commander = moveit_commander self.move_distance = 0.1 self.back_distance = 0.15
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) #robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(5) self.robot_arm.set_num_planning_attempts(5) rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) def move_code(self): self.robot_arm.set_named_target("home") #go to goal state. self.robot_arm.go(wait=True) print("====== move plan go to home 1 ======") rospy.sleep(0.5) robot_state = self.robot_arm.get_current_pose() robot_angle = self.robot_arm.get_current_joint_values() print(robot_state) # print("====== move plan go to up ======") self.robot_arm.set_named_target("up") #go to goal state. self.robot_arm.go(wait=True) print("====== move plan go to zero ======") rospy.sleep(0.5) robot_state = self.robot_arm.get_current_pose() robot_angle = self.robot_arm.get_current_joint_values() print(robot_state) self.robot_arm.set_named_target("home") #go to goal state. self.robot_arm.go(wait=True) print("====== move plan go to home2 ======") rospy.sleep(0.5) # robot_arm.set_named_target("up") # robot_arm.go(wait=True) robot_state = self.robot_arm.get_current_pose() robot_angle = self.robot_arm.get_current_joint_values() print(robot_state)
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_cartesian_demo', anonymous=True) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('arm') # 当运动规划失败后,允许重新规划 arm.allow_replanning(False) # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.001) arm.set_goal_orientation_tolerance(0.01) arm.set_max_velocity_scaling_factor(0.4) arm.set_max_acceleration_scaling_factor(0.5) arm.set_planning_time(0.1) # 规划时间限制为2秒 # arm.set_num_planning_attempts(1) # 规划1次 # 获取终端link的名称 eef_link = arm.get_end_effector_link() scene = moveit_commander.PlanningSceneInterface() print "[INFO] Current pose:\n", arm.get_current_pose().pose self.box_name = '' self.scene = scene self.group = arm self.eef_link = eef_link self.reference_frame = reference_frame self.moveit_commander = moveit_commander self.broadcaster = tf.TransformBroadcaster() self.listener = tf.TransformListener() self.gripper_len = 0.082 # 手爪实际长度0.165m, 虚拟夹爪深度0.075m 0.16-0.075=0.085m self.approach_distance = 0.05 self.back_distance = 0.05
def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_test') arm = MoveGroupCommander('arm') end_effector_link = arm.get_end_effector_link() arm.allow_replanning(True) arm.set_planning_time(5) target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_init_orientation = Quaternion() target_init_orientation = quaternion_from_euler(0.0, 1.57, 0.0) self.setPose(target_pose, [0.5, 0.2, 0.5],list(target_init_orientation)) current_pose = arm.get_current_pose(end_effector_link) self.setPose(current_pose, [current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z], list(target_init_orientation)) arm.set_pose_target(current_pose) arm.go() rospy.sleep(2) constraints = Constraints() orientation_constraint = OrientationConstraint() constraints.name = 'gripper constraint' orientation_constraint.header = target_pose.header orientation_constraint.link_name = end_effector_link orientation_constraint.orientation.x = target_init_orientation[0] orientation_constraint.orientation.y = target_init_orientation[1] orientation_constraint.orientation.z = target_init_orientation[2] orientation_constraint.orientation.w = target_init_orientation[3] orientation_constraint.absolute_x_axis_tolerance = 0.03 orientation_constraint.absolute_y_axis_tolerance = 0.03 orientation_constraint.absolute_z_axis_tolerance = 3.14 orientation_constraint.weight = 1.0 constraints.orientation_constraints.append(orientation_constraint) arm.set_path_constraints(constraints) arm.set_pose_target(target_pose, end_effector_link) arm.go() rospy.sleep(1) moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def setUpClass(self): rospy.init_node('test_moveit') rospy.sleep(5) # intentinally wait for starting up hrpsys self.robot = RobotCommander() # TODO: Read groups from SRDF file ideally. for mg_attr in self._MOVEGROUP_ATTRS: mg = MoveGroupCommander(mg_attr[0]) # Temporary workaround of planner's issue similar to https://github.com/tork-a/rtmros_nextage/issues/170 mg.set_planner_id(self._KINEMATICSOLVER_SAFE) # Allow replanning to increase the odds of a solution mg.allow_replanning(True) # increase planning time mg.set_planning_time(30.0) # Append MoveGroup instance to the MoveGroup attribute list. mg_attr.append(mg)
def __init__(self): ## Instantiate a `MoveGroupCommander`_ object. This object is an interface ## to a planning group (group of joints). move_group = MoveGroupCommander("panda_arm_hand") move_group_arm = MoveGroupCommander("panda_arm") move_group_arm.set_planning_time(45) ## Instantiate a `RobotCommander`_ object. Provides information such as the robot's ## kinematic model and the robot's current joint states robot = RobotCommander() #end effector link eef_link = move_group_arm.get_end_effector_link() # Misc variables self.robot = robot self.move_group= move_group self.move_group_arm= move_group_arm self.eef_link=eef_link
def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('cobra_move_position', anonymous=True) arm = MoveGroupCommander(ARM_GROUP) arm.allow_replanning(True) arm.set_planning_time(10) arm.set_named_target(START_POSITION) arm.go() rospy.sleep(2) print "======== create new goal position ========" start_pose = PoseStamped() start_pose.header.frame_id = arm.get_planning_frame() # Test Position start_pose.pose.position.x = 0.2 # 20 cm start_pose.pose.position.y = -0.11 # -11 cm start_pose.pose.position.z = 0.6 # 60 cm q = quaternion_from_euler(0.0, 0.0, 0.0) start_pose.pose.orientation = Quaternion(*q) print start_pose print "====== move to position =======" # KDL # arm.set_joint_value_target(start_pose, True) # IK Fast arm.set_position_target([start_pose.pose.position.x, start_pose.pose.position.y, start_pose.pose.position.z], arm.get_end_effector_link()) arm.go() rospy.sleep(2) moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('cobra_test_position', anonymous=True) arm = MoveGroupCommander(ARM_GROUP) arm.allow_replanning(True) # max planning time arm.set_planning_time(10) # start pose arm.set_start_state_to_current_state() end_effector = arm.get_end_effector_link() rospy.sleep(1) print "======== create 100 new goal position ========" start_pose = PoseStamped() start_pose.header.frame_id = arm.get_planning_frame() i = 1 while i <= 1000: # random position start_pose = arm.get_random_pose(end_effector) q = quaternion_from_euler(0.0, 0.0, 0.0) start_pose.pose.orientation = Quaternion(*q) print "====== move to position", i, "=======" # KDL arm.set_joint_value_target(start_pose, True) # IK Fast # arm.set_position_target([start_pose.pose.position.x, start_pose.pose.position.y, # start_pose.pose.position.z], end_effector) i += 1 arm.go() rospy.sleep(2) moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_pick_and_place_demo') # Use the planning scene object to add or remove objects scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped) # Create a dictionary to hold object colors self.colors = dict() # Initialize the move group for the right arm arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) arm.set_goal_position_tolerance(0.05) arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution arm.allow_replanning(True) # Set the right arm reference frame arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt arm.set_planning_time(5)
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_cartesian_demo', anonymous=True) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('arm') robot = moveit_commander.RobotCommander() # 当运动规划失败后,允许重新规划 arm.allow_replanning(False) # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.0001) arm.set_goal_orientation_tolerance(0.0001) arm.set_max_velocity_scaling_factor(1.0) arm.set_planning_time(0.05) # 规划时间限制 # 获取终端link的名称 eef_link = arm.get_end_effector_link() print "[INFO] Current pose:", arm.get_current_pose(eef_link).pose # 控制机械臂运动到之前设置的姿态 arm.set_named_target('work') arm.go() self.group = arm self.robot = robot self.eef_link = eef_link self.reference_frame = reference_frame self.moveit_commander = moveit_commander
class PlannerAnnotationParser(AnnotationParserBase): """ Parses the annotations files that contains the benchmarking information. """ def __init__(self, path_to_annotation, path_to_data): super(PlannerAnnotationParser, self).__init__(path_to_annotation, path_to_data) self.parse() self._load_scene() self._init_planning() self.benchmark() def check_results(self, results): """ Returns the results from the planner, checking them against any eventual validation data (no validation data in our case). """ return self.planner_data def _load_scene(self): """ Loads the proper scene for the planner. It can be either a python static scene or bag containing an occupancy map. """ scene = self._annotations["scene"] for element in scene: if element["type"] == "launch": self.play_launch(element["name"]) elif element["type"] == "python": self.load_python(element["name"]) elif element["type"] == "bag": self.play_bag(element["name"]) for _ in range(150): rospy.sleep(0.3) # wait for the scene to be spawned properly rospy.sleep(0.5) def _init_planning(self): """ Initialises the needed connections for the planning. """ self.group_id = self._annotations["group_id"] self.planners = self._annotations["planners"] self.scene = PlanningSceneInterface() self.robot = RobotCommander() self.group = MoveGroupCommander(self.group_id) self._marker_pub = rospy.Publisher('/visualization_marker_array', MarkerArray, queue_size=10, latch=True) self._planning_time_sub = rospy.Subscriber( '/move_group/result', MoveGroupActionResult, self._check_computation_time) rospy.sleep(1) self.group.set_num_planning_attempts( self._annotations["planning_attempts"]) self.group.set_goal_tolerance(self._annotations["goal_tolerance"]) self.group.set_planning_time(self._annotations["planning_time"]) self.group.allow_replanning(self._annotations["allow_replanning"]) self._comp_time = [] self.planner_data = [] def benchmark(self): for test_id, test in enumerate(self._annotations["tests"]): marker_position_1 = test["start_xyz"] marker_position_2 = test["goal_xyz"] self._add_markers(marker_position_1, "Start test \n sequence", marker_position_2, "Goal") # Start planning in a given joint position joints = test["start_joints"] current = RobotState() current.joint_state.name = self.robot.get_current_state( ).joint_state.name current_joints = list( self.robot.get_current_state().joint_state.position) current_joints[0:6] = joints current.joint_state.position = current_joints self.group.set_start_state(current) joints = test["goal_joints"] for planner in self.planners: if planner == "stomp": planner = "STOMP" elif planner == "sbpl": planner = "AnytimeD*" self.planner_id = planner self.group.set_planner_id(planner) self._plan_joints( joints, self._annotations["name"] + "-test_" + str(test_id)) return self.planner_data def _add_markers(self, point, text1, point_2, text2): # add marker for start and goal pose of EE marker_array = MarkerArray() marker_1 = Marker() marker_1.header.frame_id = "world" marker_1.header.stamp = rospy.Time.now() marker_1.type = Marker.SPHERE marker_1.scale.x = 0.04 marker_1.scale.y = 0.04 marker_1.scale.z = 0.04 marker_1.lifetime = rospy.Duration() marker_2 = deepcopy(marker_1) marker_1.color.g = 0.5 marker_1.color.a = 1.0 marker_1.id = 0 marker_1.pose.position.x = point[0] marker_1.pose.position.y = point[1] marker_1.pose.position.z = point[2] marker_2.color.r = 0.5 marker_2.color.a = 1.0 marker_2.id = 1 marker_2.pose.position.x = point_2[0] marker_2.pose.position.y = point_2[1] marker_2.pose.position.z = point_2[2] marker_3 = Marker() marker_3.header.frame_id = "world" marker_3.header.stamp = rospy.Time.now() marker_3.type = Marker.TEXT_VIEW_FACING marker_3.scale.z = 0.10 marker_3.lifetime = rospy.Duration() marker_4 = deepcopy(marker_3) marker_3.text = text1 marker_3.id = 2 marker_3.color.g = 0.5 marker_3.color.a = 1.0 marker_3.pose.position.x = point[0] marker_3.pose.position.y = point[1] marker_3.pose.position.z = point[2] + 0.15 marker_4.text = text2 marker_4.id = 3 marker_4.color.r = 0.5 marker_4.color.a = 1.0 marker_4.pose.position.x = point_2[0] marker_4.pose.position.y = point_2[1] marker_4.pose.position.z = point_2[2] + 0.15 marker_array.markers = [marker_1, marker_2, marker_3, marker_4] self._marker_pub.publish(marker_array) rospy.sleep(1) def _plan_joints(self, joints, test_name): # plan to joint target and determine success self.group.clear_pose_targets() group_variable_values = self.group.get_current_joint_values() group_variable_values[0:6] = joints[0:6] self.group.set_joint_value_target(group_variable_values) plan = self.group.plan() plan_time = "N/A" total_joint_rotation = "N/A" comp_time = "N/A" plan_success = self._check_plan_success(plan) if plan_success: plan_time = self._check_plan_time(plan) total_joint_rotation = self._check_plan_total_rotation(plan) while not self._comp_time: rospy.sleep(0.5) comp_time = self._comp_time.pop(0) self.planner_data.append([ self.planner_id, test_name, str(plan_success), plan_time, total_joint_rotation, comp_time ]) @staticmethod def _check_plan_success(plan): if len(plan.joint_trajectory.points) > 0: return True else: return False @staticmethod def _check_plan_time(plan): # find duration of successful plan number_of_points = len(plan.joint_trajectory.points) time = plan.joint_trajectory.points[number_of_points - 1].time_from_start.to_sec() return time @staticmethod def _check_plan_total_rotation(plan): # find total joint rotation in successful trajectory angles = [0, 0, 0, 0, 0, 0] number_of_points = len(plan.joint_trajectory.points) for i in range(number_of_points - 1): angles_temp = [ abs(x - y) for x, y in zip(plan.joint_trajectory.points[i + 1].positions, plan.joint_trajectory.points[i].positions) ] angles = [x + y for x, y in zip(angles, angles_temp)] total_angle_change = sum(angles) return total_angle_change def _check_computation_time(self, msg): # get computation time for successful plan to be found if msg.status.status == 3: self._comp_time.append(msg.result.planning_time) def _check_path_length(self, plan): # find distance travelled by end effector # not yet implemented return
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) # Initialize the ROS node rospy.init_node('moveit_demo', anonymous=True) robot = RobotCommander() # Connect to the right_arm move group right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Increase the planning time since constraint planning can take a while right_arm.set_planning_time(15) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the right arm reference frame right_arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow some leeway in position(meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.05) right_arm.set_goal_orientation_tolerance(0.1) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Start in the "resting" configuration stored in the SRDF file right_arm.set_named_target('right_arm_zero') # Plan and execute a trajectory to the goal configuration right_arm.go() rospy.sleep(1) # Open the gripper right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Set an initial target pose with the arm up and to the right target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.237012590198 target_pose.pose.position.y = -0.0747191267505 target_pose.pose.position.z = 0.901578401949 target_pose.pose.orientation.w = 1.0 # Set the start state and target pose, then plan and execute right_arm.set_start_state(robot.get_current_state()) right_arm.set_pose_target(target_pose, end_effector_link) right_arm.go() rospy.sleep(2) # Close the gripper right_gripper.set_joint_value_target(GRIPPER_CLOSED) right_gripper.go() rospy.sleep(1) # Store the current pose start_pose = right_arm.get_current_pose(end_effector_link) # Create a contraints list and give it a name constraints = Constraints() constraints.name = "Keep gripper horizontal" # Create an orientation constraint for the right gripper orientation_constraint = OrientationConstraint() orientation_constraint.header = start_pose.header orientation_constraint.link_name = right_arm.get_end_effector_link() orientation_constraint.orientation.w = 1.0 orientation_constraint.absolute_x_axis_tolerance = 0.1 orientation_constraint.absolute_y_axis_tolerance = 0.1 orientation_constraint.absolute_z_axis_tolerance = 3.14 orientation_constraint.weight = 1.0 # Append the constraint to the list of contraints constraints.orientation_constraints.append(orientation_constraint) # Set the path constraints on the right_arm right_arm.set_path_constraints(constraints) # Set a target pose for the arm target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.173187824708 target_pose.pose.position.y = -0.0159929871606 target_pose.pose.position.z = 0.692596608605 target_pose.pose.orientation.w = 1.0 # Set the start state and target pose, then plan and execute right_arm.set_start_state_to_current_state() right_arm.set_pose_target(target_pose, end_effector_link) right_arm.go() rospy.sleep(1) # Clear all path constraints right_arm.clear_path_constraints() # Open the gripper right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Return to the "resting" configuration stored in the SRDF file right_arm.set_named_target('right_arm_zero') # Plan and execute a trajectory to the goal configuration right_arm.go() rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit MoveIt moveit_commander.os._exit(0)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Construct the initial scene object scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) # Create a dictionary to hold object colors self.colors = dict() # Pause for the scene to get ready rospy.sleep(1) # Initialize the move group for the right arm right_arm = MoveGroupCommander('right_arm') # Initialize the move group for the left arm left_arm = MoveGroupCommander('left_arm') right_arm.set_planner_id("KPIECEkConfigDefault"); left_arm.set_planner_id("KPIECEkConfigDefault"); rospy.sleep(1) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.01) right_arm.set_goal_orientation_tolerance(0.05) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the reference frame for pose targets reference_frame = 'base_footprint' # Set the right arm reference frame accordingly right_arm.set_pose_reference_frame(reference_frame) # Allow 5 seconds per planning attempt right_arm.set_planning_time(5) # Give each of the scene objects a unique name table_id = 'table' box1_id = 'box1' box2_id = 'box2' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "resting" pose stored in the SRDF file #left_arm.set_named_target('left_start') #left_arm.go() # Start the arm in the "resting" pose stored in the SRDF file right_arm.set_named_target('right_start') right_arm.go() rospy.sleep(2) # Set the height of the table off the ground table_ground = 0.75 # Set the length, width and height of the table and boxes table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # Add a table top and two boxes to the scene table_pose = PoseStamped() table_pose.header.frame_id = reference_frame table_pose.pose.position.x = 0.56 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose = PoseStamped() box1_pose.header.frame_id = reference_frame box1_pose.pose.position.x = 0.51 box1_pose.pose.position.y = -0.1 box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0 box1_pose.pose.orientation.w = 1.0 scene.add_box(box1_id, box1_pose, box1_size) box2_pose = PoseStamped() box2_pose.header.frame_id = reference_frame box2_pose.pose.position.x = 0.49 box2_pose.pose.position.y = 0.15 box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 scene.add_box(box2_id, box2_pose, box2_size) # Make the table red and the boxes orange self.setColor(table_id, 0.8, 0, 0, 1.0) self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the target pose in between the boxes and above the table target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.pose.position.x = 0.4 target_pose.pose.position.y = 0.0 target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05 target_pose.pose.orientation.w = 1.0 # Set the target pose for the arm right_arm.set_pose_target(target_pose, end_effector_link) # Move the arm to the target pose (if possible) right_arm.go() # Pause for a moment... rospy.sleep(2) # Return the arm to the "resting" pose stored in the SRDF file right_arm.set_named_target('right_start') right_arm.go() # Exit MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Use the planning scene object to add or remove objects scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=5) # Create a dictionary to hold object colors self.colors = dict() # Initialize the move group for the right arm right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.05) right_arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the right arm reference frame right_arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt right_arm.set_planning_time(60) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 5 # Set a limit on the number of place attempts max_place_attempts = 5 # Give the scene a chance to catch up rospy.sleep(2) # Give each of the scene objects a unique name table_id = 'table' box1_id = 'box1' box2_id = 'box2' target_id = 'target' tool_id = 'tool' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_world_object(target_id) scene.remove_world_object(tool_id) # Remove any attached objects from a previous session scene.remove_attached_object(GRIPPER_FRAME, target_id) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "grasp" pose stored in the SRDF file right_arm.set_named_target('right_arm_up') right_arm.go() # Open the gripper to the neutral position right_gripper.set_joint_value_target(GRIPPER_OPEN) right_gripper.go() rospy.sleep(5) # Set the height of the table off the ground table_ground = 0.04 # Set the dimensions of the scene objects [l, w, h] table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # Set the target size [l, w, h] target_size = [0.02, 0.01, 0.12] target_x = 0.135 #target_y = -0.32 target_y = -0.285290879999 # Add a table top and two boxes to the scene table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose.position.x = 0.25 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) # Set the target pose in between the boxes and on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = target_x target_pose.pose.position.y = target_y target_pose.pose.position.z = table_ground + table_size[ 2] + target_size[2] / 2.0 target_pose.pose.orientation.w = 1.0 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) # Make the table blue and the boxes orange self.setColor(table_id, 0, 0, 0.8, 1.0) self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # Make the target yellow self.setColor(target_id, 0.9, 0.9, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the support surface name to the table object right_arm.set_support_surface_name(table_id) # Specify a pose to place the target after being picked up place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = 0.18 place_pose.pose.position.y = 0 place_pose.pose.position.z = table_ground + table_size[ 2] + target_size[2] / 2.0 place_pose.pose.orientation.w = 1.0 p = PoseStamped() p.header.frame_id = "up1_footprint" p.pose.position.x = 0.12792118579 p.pose.position.y = -0.285290879999 p.pose.position.z = 0.120301181892 p.pose.orientation.x = 0.0 p.pose.orientation.y = 0.0 p.pose.orientation.z = -0.706825181105 p.pose.orientation.w = 0.707388269167 right_gripper.set_pose_target(p.pose) # pick an object right_arm.allow_replanning(True) right_arm.allow_looking(True) right_arm.set_goal_tolerance(0.05) right_arm.set_planning_time(60) print "arm grasp" success = 0 attempt = 0 while not success: p_plan = right_arm.plan() attempt = attempt + 1 print "Planning attempt: " + str(attempt) if p_plan.joint_trajectory.points != []: success = 1 print "arm grasp" right_arm.execute(p_plan) rospy.sleep(5) right_gripper.set_joint_value_target(GRIPPER_GRASP) right_gripper.go() print "gripper closed" rospy.sleep(5) scene.attach_box(GRIPPER_FRAME, target_id) print "object attached" right_arm.set_named_target('right_arm_up') right_arm.go() print "arm up" rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) self.detected = {} self.detected_names = rospy.get_param('/darknet_ros/yolo_model/detection_classes/names') self.object_pose_sub = rospy.Subscriber('/cluster_decomposer/centroid_pose_array',PoseArray,self.collectJsk) self.listener = tf.TransformListener() # self.object_name_sub = rospy.Subscriber('/darknet_ros/bounding_boxes',BoundingBoxes,self.collect) self.tomatoboundingBox = BoundingBox() display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory,queue_size=20) global goal_x global goal_y global goal_z global goal_roll global goal_pitch global goal_yaw self.distance = 0 self.trans = [0.0, 0.0, 0.0] # self.marker = [] self.tomato = [] self.position_list = [] self.orientation_list = [] self.tomato_pose = Pose() self.goalPoseFromTomato = Pose() self.br = tf.TransformBroadcaster() self.calculated_tomato = Pose() self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(5) self.robot_arm.set_num_planning_attempts(5) self.display_trajectory_publisher = display_trajectory_publisher rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) def move_code(self): planning_frame = self.robot_arm.get_planning_frame() print ("====== planning frame: ", planning_frame) self.wpose = self.robot_arm.get_current_pose() print ("====== current pose : ", self.wpose) joint_goal = [-0.535054565144069, -2.009213503260451, 1.8350906250920112, -0.7794355413099039, -0.7980899690645948, 0.7782740454087982] print ("INIT POSE: ", self.robot_arm.get_current_pose().pose.position) self.robot_arm.go(joint_goal, wait = True) def go_to_desired_coordinate(self): self.calculated_tomato.position.x = goal_x self.calculated_tomato.position.y = goal_y self.calculated_tomato.position.z = goal_z self.calculated_tomato.orientation.x = goal_roll self.calculated_tomato.orientation.y = goal_pitch self.calculated_tomato.orientation.z = goal_yaw tf_display_position = [self.calculated_tomato.position.x, self.calculated_tomato.position.y, self.calculated_tomato.position.z] tf_display_orientation = [self.calculated_tomato.orientation.x, self.calculated_tomato.orientation.y, self.calculated_tomato.orientation.z, self.calculated_tomato.orientation.w] ii = 0 while ii < 5: ii += 1 self.br.sendTransform( tf_display_position, tf_display_orientation, rospy.Time.now(), "goal_wpose", "world") rate.sleep() ## ## ## show how to move on the Rviz tomato_id_goal_waypoints = [] tomato_id_goal_waypoints.append(copy.deepcopy(calculated_tomato)) (tomato_id_goal_plan, tomato_id_goal_fraction) = self.robot_arm.compute_cartesian_path(tomato_id_goal_waypoints, 0.01, 0.0) self.display_trajectory(tomato_id_goal_plan) print ("============ Press `Enter` to if plan is correct!! ...") raw_input() self.robot_arm.go(True) def display_trajectory(self, plan): display_trajectory_publisher = self.display_trajectory_publisher display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = self.robot_cmd.get_current_state() display_trajectory.trajectory.append(plan) display_trajectory_publisher.publish(display_trajectory) def plan_cartesian_path(self, x_offset, y_offset, z_offset, scale = 1.0): waypoints = [] ii = 1 self.wpose = self.robot_arm.get_current_pose().pose self.wpose.position.x = (scale * self.wpose.position.x) + x_offset # - 0.10 waypoints.append(copy.deepcopy(self.wpose)) ii += 1 self.wpose.position.y = (scale * self.wpose.position.y) + y_offset # + 0.05 waypoints.append(copy.deepcopy(self.wpose)) ii += 1 self.wpose.position.z = (scale * self.wpose.position.z) + z_offset waypoints.append(copy.deepcopy(self.wpose)) ii += 1 (plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0) return plan, fraction def plan_cartesian_x(self, x_offset, scale=1.0): waypoints = [] self.wpose = self.robot_arm.get_current_pose().pose self.wpose.position.x = (scale * self.wpose.position.x) + x_offset # -0.10 waypoints.append(copy.deepcopy(self.wpose)) (plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0) return plan, fraction def plan_cartesian_y(self, y_offset, scale=1.0): waypoints = [] self.wpose = self.robot_arm.get_current_pose().pose self.wpose.position.y = (scale * self.wpose.position.y) + y_offset # -0.10 waypoints.append(copy.deepcopy(self.wpose)) (plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0) return plan, fraction def plan_cartesian_z(self, z_offset, scale=1.0): waypoints = [] self.wpose = self.robot_arm.get_current_pose().pose self.wpose.position.z = (scale * self.wpose.position.z) + z_offset # -0.10 waypoints.append(copy.deepcopy(self.wpose)) (plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0) return plan, fraction def execute_plan(self, plan): group = self.robot_arm group.execute(plan, wait=True) def pose_subscriber(self): rospy.loginfo("waiting for pose topic...") rospy.get_param('/darknet_ros/yolo_model/detection_classes/names') rospy.Subscriber('/cluster_decomposer/centroid_pose_array',PoseArray,self.collectJsk) # rospy.Subscriber('/darknet_ros/bounding_boxes',BoundingBoxes,self.collect) def collectJsk(self, msg): global goal_x global goal_y global goal_z global goal_roll global goal_pitch global goal_yaw try: (trans, rot) = self.listener.lookupTransform('base_link','yolo_output00', rospy.Time(0)) goal_x = round(trans[0],2) goal_y = round(trans[1],2) goal_z = round(trans[2],2) print("====== trans [x, y, z]: ", trans) print("====== rotat [x, y, z, w]: ", rot) orientation = euler_from_quaternion(rot) goal_roll = orientation[0] goal_pitch = orientation[1] goal_yaw = orientation[2] except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.logwarn('there is no tf')
class SrRobotCommander(object): """ Base class for hand and arm commanders """ def __init__(self, name): """ Initialize MoveGroupCommander object @param name - name of the MoveIt group """ self._name = name self._move_group_commander = MoveGroupCommander(name) self._robot_commander = RobotCommander() self._robot_name = self._robot_commander._r.get_robot_name() self.refresh_named_targets() self._warehouse_name_get_srv = rospy.ServiceProxy( "get_robot_state", GetState) self._planning_scene = PlanningSceneInterface() self._joint_states_lock = threading.Lock() self._joint_states_listener = \ rospy.Subscriber("joint_states", JointState, self._joint_states_callback, queue_size=1) self._joints_position = {} self._joints_velocity = {} self._joints_effort = {} self._joints_state = None self._clients = {} self.__plan = None self._controllers = {} rospy.wait_for_service('compute_ik') self._compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK) self._forward_k = rospy.ServiceProxy('compute_fk', GetPositionFK) controller_list_param = rospy.get_param("/move_group/controller_list") # create dictionary with name of controllers and corresponding joints self._controllers = { item["name"]: item["joints"] for item in controller_list_param } self._set_up_action_client(self._controllers) self.tf_buffer = tf2_ros.Buffer() self.listener = tf2_ros.TransformListener(self.tf_buffer) threading.Thread(None, rospy.spin) def set_planner_id(self, planner_id): self._move_group_commander.set_planner_id(planner_id) def set_num_planning_attempts(self, num_planning_attempts): self._move_group_commander.set_num_planning_attempts( num_planning_attempts) def set_planning_time(self, seconds): self._move_group_commander.set_planning_time(seconds) def get_end_effector_pose_from_named_state(self, name): state = self._warehouse_name_get_srv(name, self._robot_name).state return self.get_end_effector_pose_from_state(state) def get_end_effector_pose_from_state(self, state): header = Header() fk_link_names = [self._move_group_commander.get_end_effector_link()] header.frame_id = self._move_group_commander.get_pose_reference_frame() response = self._forward_k(header, fk_link_names, state) return response.pose_stamped[0] def get_planning_frame(self): return self._move_group_commander.get_planning_frame() def set_pose_reference_frame(self, reference_frame): self._move_group_commander.set_pose_reference_frame(reference_frame) def get_group_name(self): return self._name def refresh_named_targets(self): self._srdf_names = self.__get_srdf_names() self._warehouse_names = self.__get_warehouse_names() def set_max_velocity_scaling_factor(self, value): self._move_group_commander.set_max_velocity_scaling_factor(value) def set_max_acceleration_scaling_factor(self, value): self._move_group_commander.set_max_acceleration_scaling_factor(value) def allow_looking(self, value): self._move_group_commander.allow_looking(value) def allow_replanning(self, value): self._move_group_commander.allow_replanning(value) def execute(self): """ Executes the last plan made. """ if self.check_plan_is_valid(): self._move_group_commander.execute(self.__plan) self.__plan = None else: rospy.logwarn("No plans were made, not executing anything.") def execute_plan(self, plan): if self.check_given_plan_is_valid(plan): self._move_group_commander.execute(plan) self.__plan = None else: rospy.logwarn("Plan is not valid, not executing anything.") def move_to_joint_value_target(self, joint_states, wait=True, angle_degrees=False): """ Set target of the robot's links and moves to it. @param joint_states - dictionary with joint name and value. It can contain only joints values of which need to be changed. @param wait - should method wait for movement end or not @param angle_degrees - are joint_states in degrees or not """ joint_states_cpy = copy.deepcopy(joint_states) if angle_degrees: joint_states_cpy.update( (joint, radians(i)) for joint, i in joint_states_cpy.items()) self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_joint_value_target(joint_states_cpy) self._move_group_commander.go(wait=wait) def plan_to_joint_value_target(self, joint_states, angle_degrees=False): """ Set target of the robot's links and plans. @param joint_states - dictionary with joint name and value. It can contain only joints values of which need to be changed. @param angle_degrees - are joint_states in degrees or not This is a blocking method. """ joint_states_cpy = copy.deepcopy(joint_states) if angle_degrees: joint_states_cpy.update( (joint, radians(i)) for joint, i in joint_states_cpy.items()) self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_joint_value_target(joint_states_cpy) self.__plan = self._move_group_commander.plan() return self.__plan def check_plan_is_valid(self): """ Checks if current plan contains a valid trajectory """ return (self.__plan is not None and len(self.__plan.joint_trajectory.points) > 0) def check_given_plan_is_valid(self, plan): """ Checks if given plan contains a valid trajectory """ return (plan is not None and len(plan.joint_trajectory.points) > 0) def get_robot_name(self): return self._robot_name def named_target_in_srdf(self, name): return name in self._srdf_names def set_named_target(self, name): if name in self._srdf_names: self._move_group_commander.set_named_target(name) elif (name in self._warehouse_names): response = self._warehouse_name_get_srv(name, self._robot_name) active_names = self._move_group_commander._g.get_active_joints() joints = response.state.joint_state.name positions = response.state.joint_state.position js = {} for n, this_name in enumerate(joints): if this_name in active_names: js[this_name] = positions[n] self._move_group_commander.set_joint_value_target(js) else: rospy.logerr("Unknown named state '%s'..." % name) return False return True def get_named_target_joint_values(self, name): output = dict() if (name in self._srdf_names): output = self._move_group_commander.\ _g.get_named_target_values(str(name)) elif (name in self._warehouse_names): js = self._warehouse_name_get_srv( name, self._robot_name).state.joint_state for x, n in enumerate(js.name): if n in self._move_group_commander._g.get_joints(): output[n] = js.position[x] else: rospy.logerr("No target named %s" % name) return None return output def get_end_effector_link(self): return self._move_group_commander.get_end_effector_link() def get_current_pose(self, reference_frame=None): """ Get the current pose of the end effector. @param reference_frame - The desired reference frame in which end effector pose should be returned. If none is passed, it will use the planning frame as reference. @return geometry_msgs.msg.Pose() - current pose of the end effector """ if reference_frame is not None: try: trans = self.tf_buffer.lookup_transform( reference_frame, self._move_group_commander.get_end_effector_link(), rospy.Time(0), rospy.Duration(5.0)) current_pose = geometry_msgs.msg.Pose() current_pose.position.x = trans.transform.translation.x current_pose.position.y = trans.transform.translation.y current_pose.position.z = trans.transform.translation.z current_pose.orientation.x = trans.transform.rotation.x current_pose.orientation.y = trans.transform.rotation.y current_pose.orientation.z = trans.transform.rotation.z current_pose.orientation.w = trans.transform.rotation.w return current_pose except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): rospy.logwarn( "Couldn't get the pose from " + self._move_group_commander.get_end_effector_link() + " in " + reference_frame + " reference frame") return None else: return self._move_group_commander.get_current_pose().pose def get_current_state(self): """ Get the current joint state of the group being used. @return a dictionary with the joint names as keys and current joint values """ joint_names = self._move_group_commander._g.get_active_joints() joint_values = self._move_group_commander._g.get_current_joint_values() return dict(zip(joint_names, joint_values)) def get_current_state_bounded(self): """ Get the current joint state of the group being used, enforcing that they are within each joint limits. @return a dictionary with the joint names as keys and current joint values """ current = self._move_group_commander._g.get_current_state_bounded() names = self._move_group_commander._g.get_active_joints() output = {n: current[n] for n in names if n in current} return output def get_robot_state_bounded(self): return self._move_group_commander._g.get_current_state_bounded() def move_to_named_target(self, name, wait=True): """ Set target of the robot's links and moves to it @param name - name of the target pose defined in SRDF @param wait - should method wait for movement end or not """ self._move_group_commander.set_start_state_to_current_state() if self.set_named_target(name): self._move_group_commander.go(wait=wait) def plan_to_named_target(self, name): """ Set target of the robot's links and plans This is a blocking method. @param name - name of the target pose defined in SRDF """ self._move_group_commander.set_start_state_to_current_state() if self.set_named_target(name): self.__plan = self._move_group_commander.plan() def __get_warehouse_names(self): try: list_srv = rospy.ServiceProxy("list_robot_states", ListStates) return list_srv("", self._robot_name).states except rospy.ServiceException as exc: rospy.logwarn("Couldn't access warehouse: " + str(exc)) return list() def _reset_plan(self): self.__plan = None def _set_plan(self, plan): self.__plan = plan def __get_srdf_names(self): return self._move_group_commander._g.get_named_targets() def get_named_targets(self): """ Get the complete list of named targets, from SRDF as well as warehouse poses if available. @return list of strings containing names of targets. """ return self._srdf_names + self._warehouse_names def get_joints_position(self): """ Returns joints position @return - dictionary with joints positions """ with self._joint_states_lock: return self._joints_position def get_joints_velocity(self): """ Returns joints velocities @return - dictionary with joints velocities """ with self._joint_states_lock: return self._joints_velocity def _get_joints_effort(self): """ Returns joints effort @return - dictionary with joints efforts """ with self._joint_states_lock: return self._joints_effort def get_joints_state(self): """ Returns joints state @return - JointState message """ with self._joint_states_lock: return self._joints_state def run_joint_trajectory(self, joint_trajectory): """ Moves robot through all joint states with specified timeouts @param joint_trajectory - JointTrajectory class object. Represents trajectory of the joints which would be executed. """ plan = RobotTrajectory() plan.joint_trajectory = joint_trajectory self._move_group_commander.execute(plan) def make_named_trajectory(self, trajectory): """ Makes joint value trajectory from specified by named poses (either from SRDF or from warehouse) @param trajectory - list of waypoints, each waypoint is a dict with the following elements (n.b either name or joint_angles is required) - name -> the name of the way point - joint_angles -> a dict of joint names and angles - interpolate_time -> time to move from last wp - pause_time -> time to wait at this wp - degrees -> set to true if joint_angles is specified in degrees. Assumed false if absent. """ current = self.get_current_state_bounded() joint_trajectory = JointTrajectory() joint_names = current.keys() joint_trajectory.joint_names = joint_names start = JointTrajectoryPoint() start.positions = current.values() start.time_from_start = rospy.Duration.from_sec(0.001) joint_trajectory.points.append(start) time_from_start = 0.0 for wp in trajectory: joint_positions = None if 'name' in wp.keys(): joint_positions = self.get_named_target_joint_values( wp['name']) elif 'joint_angles' in wp.keys(): joint_positions = copy.deepcopy(wp['joint_angles']) if 'degrees' in wp.keys() and wp['degrees']: for joint, angle in joint_positions.iteritems(): joint_positions[joint] = radians(angle) if joint_positions is None: rospy.logerr( "Invalid waypoint. Must contain valid name for named target or dict of joint angles." ) return None new_positions = {} for n in joint_names: new_positions[n] = joint_positions[ n] if n in joint_positions else current[n] trajectory_point = JointTrajectoryPoint() trajectory_point.positions = [ new_positions[n] for n in joint_names ] current = new_positions time_from_start += wp['interpolate_time'] trajectory_point.time_from_start = rospy.Duration.from_sec( time_from_start) joint_trajectory.points.append(trajectory_point) if 'pause_time' in wp and wp['pause_time'] > 0: extra = JointTrajectoryPoint() extra.positions = trajectory_point.positions time_from_start += wp['pause_time'] extra.time_from_start = rospy.Duration.from_sec( time_from_start) joint_trajectory.points.append(extra) return joint_trajectory def send_stop_trajectory_unsafe(self): """ Sends a trajectory of all active joints at their current position. This stops the robot. """ current = self.get_current_state_bounded() trajectory_point = JointTrajectoryPoint() trajectory_point.positions = current.values() trajectory_point.time_from_start = rospy.Duration.from_sec(0.1) trajectory = JointTrajectory() trajectory.points.append(trajectory_point) trajectory.joint_names = current.keys() self.run_joint_trajectory_unsafe(trajectory) def run_named_trajectory_unsafe(self, trajectory, wait=False): """ Moves robot through trajectory specified by named poses, either from SRDF or from warehouse. Runs trajectory directly via contoller. @param trajectory - list of waypoints, each waypoint is a dict with the following elements: - name -> the name of the way point - interpolate_time -> time to move from last wp - pause_time -> time to wait at this wp """ joint_trajectory = self.make_named_trajectory(trajectory) if joint_trajectory is not None: self.run_joint_trajectory_unsafe(joint_trajectory, wait) def run_named_trajectory(self, trajectory): """ Moves robot through trajectory specified by named poses, either from SRDF or from warehouse. Runs trajectory via moveit. @param trajectory - list of waypoints, each waypoint is a dict with the following elements: - name -> the name of the way point - interpolate_time -> time to move from last wp - pause_time -> time to wait at this wp """ joint_trajectory = self.make_named_trajectory(trajectory) if joint_trajectory is not None: self.run_joint_trajectory(joint_trajectory) def move_to_position_target(self, xyz, end_effector_link="", wait=True): """ Specify a target position for the end-effector and moves to it @param xyz - new position of end-effector @param end_effector_link - name of the end effector link @param wait - should method wait for movement end or not """ self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_position_target(xyz, end_effector_link) self._move_group_commander.go(wait=wait) def plan_to_position_target(self, xyz, end_effector_link=""): """ Specify a target position for the end-effector and plans. This is a blocking method. @param xyz - new position of end-effector @param end_effector_link - name of the end effector link """ self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_position_target(xyz, end_effector_link) self.__plan = self._move_group_commander.plan() def move_to_pose_target(self, pose, end_effector_link="", wait=True): """ Specify a target pose for the end-effector and moves to it @param pose - new pose of end-effector: a Pose message, a PoseStamped message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] @param end_effector_link - name of the end effector link @param wait - should method wait for movement end or not """ self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_pose_target(pose, end_effector_link) self._move_group_commander.go(wait=wait) def plan_to_pose_target(self, pose, end_effector_link="", alternative_method=False): """ Specify a target pose for the end-effector and plans. This is a blocking method. @param pose - new pose of end-effector: a Pose message, a PoseStamped message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] @param end_effector_link - name of the end effector link @param alternative_method - use set_joint_value_target instead of set_pose_target """ self._move_group_commander.set_start_state_to_current_state() if alternative_method: self._move_group_commander.set_joint_value_target( pose, end_effector_link) else: self._move_group_commander.set_pose_target(pose, end_effector_link) self.__plan = self._move_group_commander.plan() return self.__plan def _joint_states_callback(self, joint_state): """ The callback function for the topic joint_states. It will store the received joint position, velocity and efforts information into dictionaries @param joint_state - the message containing the joints data. """ with self._joint_states_lock: self._joints_state = joint_state self._joints_position = { n: p for n, p in zip(joint_state.name, joint_state.position) } self._joints_velocity = { n: v for n, v in zip(joint_state.name, joint_state.velocity) } self._joints_effort = { n: v for n, v in zip(joint_state.name, joint_state.effort) } def _set_up_action_client(self, controller_list): """ Sets up an action client to communicate with the trajectory controller """ self._action_running = {} for controller_name in controller_list.keys(): self._action_running[controller_name] = False service_name = controller_name + "/follow_joint_trajectory" self._clients[controller_name] = SimpleActionClient( service_name, FollowJointTrajectoryAction) if self._clients[controller_name].wait_for_server( timeout=rospy.Duration(4)) is False: err_msg = 'Failed to connect to action server ({}) in 4 sec'.format( service_name) rospy.logwarn(err_msg) def move_to_joint_value_target_unsafe(self, joint_states, time=0.002, wait=True, angle_degrees=False): """ Set target of the robot's links and moves to it. @param joint_states - dictionary with joint name and value. It can contain only joints values of which need to be changed. @param time - time in s (counting from now) for the robot to reach the target (it needs to be greater than 0.0 for it not to be rejected by the trajectory controller) @param wait - should method wait for movement end or not @param angle_degrees - are joint_states in degrees or not """ # self._update_default_trajectory() # self._set_targets_to_default_trajectory(joint_states) goals = {} joint_states_cpy = copy.deepcopy(joint_states) if angle_degrees: joint_states_cpy.update( (joint, radians(i)) for joint, i in joint_states_cpy.items()) for controller in self._controllers: controller_joints = self._controllers[controller] goal = FollowJointTrajectoryGoal() goal.trajectory.joint_names = [] point = JointTrajectoryPoint() point.positions = [] for x in joint_states_cpy.keys(): if x in controller_joints: goal.trajectory.joint_names.append(x) point.positions.append(joint_states_cpy[x]) point.time_from_start = rospy.Duration.from_sec(time) goal.trajectory.points = [point] goals[controller] = goal self._call_action(goals) if not wait: return for i in self._clients.keys(): if not self._clients[i].wait_for_result(): rospy.loginfo("Trajectory not completed") def action_is_running(self, controller=None): if controller is not None: return self._action_running[controller] for controller_running in self._action_running.values(): if controller_running: return True return False def _action_done_cb(self, controller, terminal_state, result): self._action_running[controller] = False def _call_action(self, goals): for client in self._clients: self._action_running[client] = True self._clients[client].send_goal( goals[client], lambda terminal_state, result: self._action_done_cb( client, terminal_state, result)) def run_joint_trajectory_unsafe(self, joint_trajectory, wait=True): """ Moves robot through all joint states with specified timeouts @param joint_trajectory - JointTrajectory class object. Represents trajectory of the joints which would be executed. @param wait - should method wait for movement end or not """ goals = {} for controller in self._controllers: controller_joints = self._controllers[controller] goal = FollowJointTrajectoryGoal() goal.trajectory = copy.deepcopy(joint_trajectory) indices_of_joints_in_this_controller = [] for i, joint in enumerate(joint_trajectory.joint_names): if joint in controller_joints: indices_of_joints_in_this_controller.append(i) goal.trajectory.joint_names = [ joint_trajectory.joint_names[i] for i in indices_of_joints_in_this_controller ] for point in goal.trajectory.points: if point.positions: point.positions = [ point.positions[i] for i in indices_of_joints_in_this_controller ] if point.velocities: point.velocities = [ point.velocities[i] for i in indices_of_joints_in_this_controller ] if point.effort: point.effort = [ point.effort[i] for i in indices_of_joints_in_this_controller ] goals[controller] = goal self._call_action(goals) if not wait: return for i in self._clients.keys(): if not self._clients[i].wait_for_result(): rospy.loginfo("Trajectory not completed") def plan_to_waypoints_target(self, waypoints, reference_frame=None, eef_step=0.005, jump_threshold=0.0): """ Specify a set of waypoints for the end-effector and plans. This is a blocking method. @param reference_frame - the reference frame in which the waypoints are given @param waypoints - an array of poses of end-effector @param eef_step - configurations are computed for every eef_step meters @param jump_threshold - maximum distance in configuration space between consecutive points in the resulting path """ old_frame = self._move_group_commander.get_pose_reference_frame() if reference_frame is not None: self.set_pose_reference_frame(reference_frame) (self.__plan, fraction) = self._move_group_commander.compute_cartesian_path( waypoints, eef_step, jump_threshold) self.set_pose_reference_frame(old_frame) def set_teach_mode(self, teach): """ Activates/deactivates the teach mode for the robot. Activation: stops the the trajectory controllers for the robot, and sets it to teach mode. Deactivation: stops the teach mode and starts trajectory controllers for the robot. Currently this method blocks for a few seconds when called on a hand, while the hand parameters are reloaded. @param teach - bool to activate or deactivate teach mode """ if teach: mode = RobotTeachModeRequest.TEACH_MODE else: mode = RobotTeachModeRequest.TRAJECTORY_MODE self.change_teach_mode(mode, self._name) def move_to_trajectory_start(self, trajectory, wait=True): """ Make and execute a plan from the current state to the first state in an pre-existing trajectory @param trajectory - moveit_msgs/JointTrajectory @param wait - Bool to specify if movement should block untill finished. """ if len(trajectory.points) <= 0: rospy.logerr("Trajectory has no points in it, can't reverse...") return None first_point = trajectory.points[0] end_state = dict(zip(trajectory.joint_names, first_point.positions)) self.move_to_joint_value_target(end_state, wait=wait) @staticmethod def change_teach_mode(mode, robot): teach_mode_client = rospy.ServiceProxy('/teach_mode', RobotTeachMode) req = RobotTeachModeRequest() req.teach_mode = mode req.robot = robot try: resp = teach_mode_client(req) if resp.result == RobotTeachModeResponse.ERROR: rospy.logerr("Failed to change robot %s to mode %d", robot, mode) else: rospy.loginfo("Changed robot %s to mode %d Result = %d", robot, mode, resp.result) except rospy.ServiceException: rospy.logerr("Failed to call service teach_mode") def get_ik(self, target_pose, avoid_collisions=False, joint_states=None): """ Computes the inverse kinematics for a given pose. It returns a JointState @param target_pose - A given pose of type PoseStamped @param avoid_collisions - Find an IK solution that avoids collisions. By default, this is false """ service_request = PositionIKRequest() service_request.group_name = self._name service_request.ik_link_name = self._move_group_commander.get_end_effector_link( ) service_request.pose_stamped = target_pose service_request.timeout.secs = 0.5 service_request.avoid_collisions = avoid_collisions if joint_states is None: service_request.robot_state.joint_state = self.get_joints_state() else: service_request.robot_state.joint_state = joint_states try: resp = self._compute_ik(ik_request=service_request) # Check if error_code.val is SUCCESS=1 if resp.error_code.val != 1: if resp.error_code.val == -10: rospy.logerr("Unreachable point: Start state in collision") elif resp.error_code.val == -12: rospy.logerr("Unreachable point: Goal state in collision") elif resp.error_code.val == -31: rospy.logerr("Unreachable point: No IK solution") else: rospy.logerr("Unreachable point (error: %s)" % resp.error_code) return else: return resp.solution.joint_state except rospy.ServiceException, e: rospy.logerr("Service call failed: %s" % e)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Use the planning scene object to add or remove objects scene = PlanningSceneInterface("base_link") # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped) # Create a dictionary to hold object colors self.colors = dict() # Initialize the move group for the right arm right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.05) right_arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the right arm reference frame right_arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt right_arm.set_planning_time(15) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 5 # Set a limit on the number of place attempts max_place_attempts = 3 # Give the scene a chance to catch up rospy.sleep(2) # Connect to the UBR-1 find_objects action server 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") # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "resting" pose stored in the SRDF file right_arm.set_named_target('resting') right_arm.go() # Open the gripper to the neutral position right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Begin the main perception and pick-and-place loop while not rospy.is_shutdown(): # Initialize the grasping goal goal = FindGraspableObjectsGoal() # We don't use the UBR-1 grasp planner as it does not work with our gripper goal.plan_grasps = False # Send the goal request to the find_objects action server which will trigger # the perception pipeline find_objects.send_goal(goal) # Wait for a result find_objects.wait_for_result(rospy.Duration(5.0)) # The result will contain support surface(s) and objects(s) if any are detected find_result = find_objects.get_result() # Display the number of objects found rospy.loginfo("Found %d objects" % len(find_result.objects)) # Remove all previous objects from the planning scene for name in scene.getKnownCollisionObjects(): scene.removeCollisionObject(name, False) for name in scene.getKnownAttachedObjects(): scene.removeAttachedObject(name, False) scene.waitForSync() # Clear the virtual object colors scene._colors = dict() # Use the nearest object on the table as the target target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_size = None the_object = None the_object_dist = 1.0 count = -1 # Cycle through all detected objects and keep the nearest one for obj in find_result.objects: count += 1 scene.addSolidPrimitive("object%d"%count, obj.object.primitives[0], obj.object.primitive_poses[0], wait = False) # Choose the object nearest to the 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 # Get the size of the target target_size = obj.object.primitives[0].dimensions # Set the target pose target_pose.pose = obj.object.primitive_poses[0] # We want the gripper to be horizontal target_pose.pose.orientation.x = 0.0 target_pose.pose.orientation.y = 0.0 target_pose.pose.orientation.z = 0.0 target_pose.pose.orientation.w = 1.0 # Make sure we found at least one object before setting the target ID if the_object != None: target_id = "object%d"%the_object # Insert the support surface into the planning scene 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) # Get the table dimensions table_size = obj.primitives[0].dimensions # If no objects detected, try again if the_object == None or target_size is None: rospy.logerr("Nothing to grasp! try again...") continue # Wait for the scene to sync scene.waitForSync() # Set colors of the table and the object we are grabbing scene.setColor(target_id, 223.0/256.0, 90.0/256.0, 12.0/256.0) # orange scene.setColor(find_result.objects[the_object].object.support_surface, 0.3, 0.3, 0.3, 0.7) # grey scene.sendColors() # Skip pick-and-place if we are just detecting objects if args.objects: if args.once: exit(0) else: continue # Get the support surface ID support_surface = find_result.objects[the_object].object.support_surface # Set the support surface name to the table object right_arm.set_support_surface_name(support_surface) # Specify a pose to place the target after being picked up place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = target_pose.pose.position.x place_pose.pose.position.y = 0.03 place_pose.pose.position.z = table_size[2] + target_size[2] / 2.0 + 0.015 place_pose.pose.orientation.w = 1.0 # Initialize the grasp pose to the target pose grasp_pose = target_pose # Shift the grasp pose half the size of the target to center it in the gripper try: grasp_pose.pose.position.x += target_size[0] / 2.0 grasp_pose.pose.position.y -= 0.01 grasp_pose.pose.position.z += target_size[2] / 2.0 except: rospy.loginfo("Invalid object size so skipping") continue # Generate a list of grasps grasps = self.make_grasps(grasp_pose, [target_id]) # Publish the grasp poses so they can be viewed in RViz for grasp in grasps: self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) # Track success/failure and number of attempts for pick operation result = None n_attempts = 0 # Set the start state to the current state right_arm.set_start_state_to_current_state() # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: result = right_arm.pick(target_id, grasps) n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) rospy.sleep(1.0) # If the pick was successful, attempt the place operation if result == MoveItErrorCodes.SUCCESS: result = None n_attempts = 0 # Generate valid place poses places = self.make_places(place_pose) # Set the start state to the current state #right_arm.set_start_state_to_current_state() # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts: for place in places: result = right_arm.place(target_id, place) if result == MoveItErrorCodes.SUCCESS: break n_attempts += 1 rospy.loginfo("Place attempt: " + str(n_attempts)) rospy.sleep(0.2) if result != MoveItErrorCodes.SUCCESS: rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.") else: rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.") rospy.sleep(2) # Open the gripper to the neutral position right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(2) # Return the arm to the "resting" pose stored in the SRDF file right_arm.set_named_target('resting') right_arm.go() rospy.sleep(2) # Give the servos a rest arbotix_relax_all_servos() rospy.sleep(2) if args.once: # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_obstacles_demo') # 初始化场景对象 scene = PlanningSceneInterface() # 创建一个发布场景变化信息的发布者 self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) rospy.Subscriber("chatter", Float64MultiArray, callback) # 创建一个存储物体颜色的字典对象 self.colors = dict() # 等待场景准备就绪 rospy.sleep(1) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('arm') gripper = MoveGroupCommander('gripper') # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.01) gripper.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.2) # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) # 设置每次运动规划的时间限制:5s arm.set_planning_time(5) # 设置场景物体的名称 table_id = 'table' # cy_id = 'cy' box1_id = 'box1' box2_id = 'box2' box3_id = 'box3' sphere_id = 'sphere' # 移除场景中之前运行残留的物体 scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_world_object(box3_id) scene.remove_world_object(sphere_id) rospy.sleep(1) #控制机械臂先回到初始化位置 arm.set_named_target('init') arm.go() rospy.sleep(1) # arm.set_named_target('start') # arm.go() # rospy.sleep(1) gripper.set_joint_value_target([0.05]) gripper.go() rospy.sleep(0) # 设置桌面的高度 table_ground = 0.37 # 设置table、box1和box2的三维尺寸 table_size = [0.2, 0.3, 0.01] box1_size = [0.01, 0.01, 0.19] box2_size = [0.01, 0.01, 0.19] box3_size = [0.005, 0.01, 0.3] sphere_R = 0.01 error = 0.03 # 将三个物体加入场景当中 table_pose = PoseStamped() table_pose.header.frame_id = reference_frame table_pose.pose.position.x = -table_size[0] / 2.0 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) #scene.add_cylinder box1_pose = PoseStamped() box1_pose.header.frame_id = reference_frame box1_pose.pose.position.x = -0.09 box1_pose.pose.position.y = table_size[0] / 2.0 box1_pose.pose.position.z = 0.18 + box1_size[2] / 2.0 box1_pose.pose.orientation.w = 1.0 scene.add_box(box1_id, box1_pose, box1_size) box2_pose = PoseStamped() box2_pose.header.frame_id = reference_frame box2_pose.pose.position.x = -0.09 box2_pose.pose.position.y = -table_size[0] / 2.0 box2_pose.pose.position.z = 0.18 + box1_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 scene.add_box(box2_id, box2_pose, box2_size) # box3_pose = PoseStamped() # box3_pose.header.frame_id = reference_frame # box3_pose.pose.position.x = pos_aim[0] # box3_pose.pose.position.y = pos_aim[1] # box3_pose.pose.position.z = box3_size[2]/2.0+0.1 # box3_pose.pose.orientation.w = 1.0 # scene.add_box(box3_id, box3_pose, box3_size) sphere_pose = PoseStamped() sphere_pose.header.frame_id = reference_frame sphere_pose.pose.position.x = pos_aim[0] + 0 sphere_pose.pose.position.y = pos_aim[1] sphere_pose.pose.position.z = pos_aim[2] sphere_pose.pose.orientation.w = 1.0 scene.add_sphere(sphere_id, sphere_pose, sphere_R) # 将桌子设置成红色,两个box设置成橙色 self.setColor(table_id, 0, 0, 0, 1) # self.setColor(table_id, 0.8, 0, 0, 1.0) # self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box1_id, 1, 1, 1, 1.0) self.setColor(box2_id, 1, 1, 1, 1.0) self.setColor(box3_id, 1, 1, 1, 1.0) self.setColor(sphere_id, 0.8, 0, 0, 1.0) # 将场景中的颜色设置发布 self.sendColors() # rospy.INFO("waiting...") # if(Recive_FLAG==1): # rospy.INFO("OK!") # 设置机械臂的运动目标位置 target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.pose.position.x = pos_aim[0] - error target_pose.pose.position.y = pos_aim[1] target_pose.pose.position.z = pos_aim[2] # target_pose.pose.orientation.w = 1.0 #####0.3 # 0.2 0.2 0.2 0.15 0.15 # 0.12 0.11 0.12 0.15 0.15 # 0.30 0.245 0.35 0.35 0.25 # 控制机械臂运动到目标位置 arm.set_pose_target(target_pose, end_effector_link) arm.go() rospy.sleep(1) gripper.set_joint_value_target([0.03]) gripper.go() rospy.sleep(1) # # 控制机械臂终端向x移动5cm arm.shift_pose_target(0, 0.01, end_effector_link) arm.go() rospy.sleep(1) gripper.set_joint_value_target([0.05]) gripper.go() rospy.sleep(1) # # 设置机械臂的运动目标位置,进行避障规划 # target_pose2 = PoseStamped() # target_pose2.header.frame_id = reference_frame # target_pose2.pose.position.x = 0.15 # target_pose2.pose.position.y = 0 #-0.25 # target_pose2.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05 # target_pose2.pose.orientation.w = 1.0 # # 控制机械臂运动到目标位置 # arm.set_pose_target(target_pose2, end_effector_link) # arm.go() # rospy.sleep(2) #控制机械臂回到初始化位置 arm.set_named_target('init') arm.go() # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0) rospy.spin()
# modify the pose p.pose.position.x = 0.72 p.pose.position.z = 0.05 # add the pen scene.add_mesh("pen",p,resourcepath+'objects/pen.dae') rospy.sleep(1) # print the existing groups robot = RobotCommander() print "Available groups: ",robot.get_group_names() # setup the arm group and its planner arm = MoveGroupCommander("arm") arm.set_start_state_to_current_state() arm.set_planner_id("RRTstarkConfigDefault") arm.set_planning_time(5.0) arm.detach_object("pen") # set the arm to a safe target arm.set_named_target("gamma") # plan and execute the motion arm.go() # setup the hand group and its planner hand = MoveGroupCommander("hand") hand.set_start_state_to_current_state() hand.set_planner_id("LBKPIECEkConfigDefault") hand.set_planning_time(10.0) # set the hand to a safe target hand.set_named_target("open")
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_obstacles_demo') # Construct the initial scene object scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) # Create a dictionary to hold object colors self.colors = dict() # Pause for the scene to get ready rospy.sleep(1) # Initialize the move group for the right arm arm = MoveGroupCommander(GROUP_NAME_ARM) # Get the name of the end-effector link end_effector_link = arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) arm.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.05) # Allow replanning to increase the odds of a solution arm.allow_replanning(True) # Set the right arm reference frame accordingly arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt arm.set_planning_time(5) # Give each of the scene objects a unique name table_id = 'table' box1_id = 'box1' box2_id = 'box2' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "resting" pose stored in the SRDF file arm.set_named_target('l_arm_init') arm.go() rospy.sleep(2) # Set the height of the table off the ground table_ground = 0.65 # Set the length, width and height of the table and boxes table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # Add a table top and two boxes to the scene table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose.position.x = 0.35 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose = PoseStamped() box1_pose.header.frame_id = REFERENCE_FRAME box1_pose.pose.position.x = 0.3 box1_pose.pose.position.y = 0 box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0 box1_pose.pose.orientation.w = 1.0 scene.add_box(box1_id, box1_pose, box1_size) box2_pose = PoseStamped() box2_pose.header.frame_id = REFERENCE_FRAME box2_pose.pose.position.x = 0.3 box2_pose.pose.position.y = 0.25 box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 scene.add_box(box2_id, box2_pose, box2_size) # Make the table red and the boxes orange self.setColor(table_id, 0.8, 0, 0, 1.0) self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the target pose in between the boxes and above the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.22 target_pose.pose.position.y = 0.14 target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05 q = quaternion_from_euler(0, 0, -1.57079633) target_pose.pose.orientation.x = q[0] target_pose.pose.orientation.y = q[1] target_pose.pose.orientation.z = q[2] target_pose.pose.orientation.w = q[3] # Set the target pose for the arm arm.set_pose_target(target_pose, end_effector_link) # Move the arm to the target pose (if possible) arm.go() # Pause for a moment... rospy.sleep(2) # Return the arm to the "resting" pose stored in the SRDF file arm.set_named_target('l_arm_init') arm.go() # Exit MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') #Initialize robot robot = moveit_commander.RobotCommander() # Use the planning scene object to add or remove objects scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10) # Create a publisher for displaying object frames self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10) ## Create a publisher for visualizing direction ### self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10) # Create a dictionary to hold object colors self.colors = dict() # Initialize the MoveIt! commander for the arm right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the MoveIt! commander for the gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Allow 5 seconds per planning attempt right_arm.set_planning_time(5) # Prepare Gripper and open it self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction) self.ac.wait_for_server() g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100)) g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.0, 1)) self.ac.send_goal(g_open) # Give the scene a chance to catch up rospy.sleep(2) # Prepare Gazebo Subscriber self.pwh = None self.pwh_copy = None self.idx_targ = None self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback) rospy.sleep(2) # PREPARE THE SCENE while self.pwh is None: rospy.sleep(0.05) target_id = 'target' self.taid = self.pwh.name.index('wood_cube_5cm') table_id = 'table' self.tid = self.pwh.name.index('table') #obstacle1_id = 'obstacle1' #self.o1id = self.pwh.name.index('wood_block_10_2_1cm') # Remove leftover objects from a previous run scene.remove_world_object(target_id) scene.remove_world_object(table_id) #scene.remove_world_object(obstacle1_id) # Remove any attached objects from a previous session scene.remove_attached_object(GRIPPER_FRAME, target_id) # Set the target size [l, w, h] target_size = [0.05, 0.05, 0.05] table_size = [1.5, 0.8, 0.03] #obstacle1_size = [0.1, 0.025, 0.01] ## Set the target pose on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose = self.pwh.pose[self.taid] target_pose.pose.position.z += 0.025 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose = self.pwh.pose[self.tid] table_pose.pose.position.z += 1 scene.add_box(table_id, table_pose, table_size) #obstacle1_pose = PoseStamped() #obstacle1_pose.header.frame_id = REFERENCE_FRAME #obstacle1_pose.pose = self.pwh.pose[self.o1id] ## Add the target object to the scene #scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size) # Specify a pose to place the target after being picked up place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = 0.50 place_pose.pose.position.y = -0.30 place_pose.pose.orientation.w = 1.0 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) ### Make the target purple ### self.setColor(target_id, 0.6, 0, 1, 1.0) # Send the colors to the planning scene self.sendColors() #print target_pose self.object_frames_pub.publish(target_pose) rospy.sleep(2) print "==================== Generating Transformations ===========================" M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]) M1[0,3] = target_pose.pose.position.x M1[1,3] = target_pose.pose.position.y M1[2,3] = target_pose.pose.position.z M2 = transformations.euler_matrix(0, 1.57, 0) M2[0,3] = 0.0 # offset about x M2[1,3] = 0.0 # about y M2[2,3] = 0.25 # about z T = np.dot(M1, M2) pre_grasping = deepcopy(target_pose) pre_grasping.pose.position.x = T[0,3] pre_grasping.pose.position.y = T[1,3] pre_grasping.pose.position.z = T[2,3] quat = transformations.quaternion_from_matrix(T) pre_grasping.pose.orientation.x = quat[0] pre_grasping.pose.orientation.y = quat[1] pre_grasping.pose.orientation.z = quat[2] pre_grasping.pose.orientation.w = quat[3] pre_grasping.header.frame_id = 'gazebo_world' # # Initialize the grasp object # g = Grasp() # grasps = [] # # Set the first grasp pose to the input pose # g.grasp_pose = pre_grasping # g.allowed_touch_objects = [target_id] # grasps.append(deepcopy(g)) # right_arm.pick(target_id, grasps) #Change the frame_id for the planning to take place! #target_pose.header.frame_id = 'gazebo_world' self.p_pub.publish(pre_grasping) right_arm.set_pose_target(pre_grasping, GRIPPER_FRAME) plan = right_arm.plan() rospy.sleep(2) right_arm.go(wait=True) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
class Arm_Controller: def __init__(self): # Give the launch a chance to catch up # rospy.sleep(5) # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('Arm_Controller') rospy.loginfo("Launched Arm Controller") # constants self.GROUP_NAME_ARM = 'arm' self.GRIPPER_FRAME = 'gripper_link' self.REFERENCE_FRAME = 'base_link' self.ARM_BASE_FRAME = 'arm_base_link' self.done = True self.test_pose_publisher = rospy.Publisher('/test_arm_pose', PoseStamped) rospy.Subscriber("/arm_target_pose", PoseStamped, self.move_arm_to_pose, queue_size=1) self.robot_name = "gatlin" move_arm_service = createService('gatlin/move/arm', MoveRobot, self.handle_move_arm) # We need a tf listener to convert poses into arm reference base self.tfl = tf.TransformListener() # Initialize the move group for the right arm self.arm = MoveGroupCommander(self.GROUP_NAME_ARM) self.gripper = Gripper() self.robot = moveit_commander.RobotCommander() # Allow replanning to increase the odds of a solution self.arm.allow_replanning(True) # Set the planner self.arm.set_planner_id("RRTConnectkConfigDefault") # Set the right arm reference frame self.arm.set_pose_reference_frame(self.REFERENCE_FRAME) # Give the scene a chance to catch up rospy.sleep(1) # Allow some leeway in position (meters) and orientation (radians) # USELESS; do not work on pick and place! Explained on this issue: # https://github.com/ros-planning/moveit_ros/issues/577 self.arm.set_goal_position_tolerance(0.005) self.arm.set_goal_orientation_tolerance(0.05) # Allow 2 seconds per planning attempt self.arm.set_planning_time(2.0) # Create a quaternion from the Euler angles #q = quaternion_from_euler(0, pitch, yaw) # horiz = Quaternion(-0.023604, 0.99942, 0.00049317, 0.024555) # deg45 = Quaternion(-0.022174, 0.9476, 0.0074215, 0.31861) down = Quaternion(-0.00035087, 0.73273, 0.00030411, 0.68052) # back_pos = Point(-0.03, 0.0313, 0.476) # init rest pose self.rest_pose = PoseStamped() self.rest_pose.header.frame_id = self.REFERENCE_FRAME self.rest_pose.pose.position = Point(-0.06, 0.00, 0.35) self.rest_pose.pose.orientation = Quaternion(0.0251355325061, 0.982948881414, -0.0046583987932, 0.182093384981) # init place_upper pose self.place_upper_pose = PoseStamped() self.place_upper_pose.header.frame_id = self.REFERENCE_FRAME self.place_upper_pose.pose.position = Point(0.24713, -0.0051618, 0.37998) self.place_upper_pose.pose.orientation = Quaternion(0.0030109, 0.1472, -0.020231, 0.9889) # init current pose self.current_pose = PoseStamped() self.current_pose.header.frame_id = self.REFERENCE_FRAME self.current_pose.pose.position = Point(0,0,0) self.current_pose.pose.orientation = down # Open the gripper rospy.loginfo("Set Gripper: open") self.gripper.set(1.0) # self.arm.set_pose_target(self.rest_pose) # self.arm.go() # rospy.sleep(1) # self.ar = ArbotixROS() # rate = rospy.Rate(30) # while not rospy.is_shutdown(): # # rospy.logerr(self.ar.getVoltage(4)) # # rospy.logerr(self.ar.getSpeed(5)) # rospy.logerr(self.ar.getPosition(1)) # rate.sleep() rospy.spin() def MoveToPoseWithIntermediate(self, ps, offsets) : success = False for offset in offsets: # interpose = getOffsetPose(hand_pose, offset) interpose = getOffsetPose(ps, offset) success = self.MoveToPose(interpose, "MoveToIntermediatePose") rospy.sleep(1) success = self.MoveToPose(ps, "MoveToPose") return success def MoveToPose(self, ps, name) : newpose = self.transform_pose(self.REFERENCE_FRAME, ps) down = Quaternion(-0.00035087, 0.73273, 0.00030411, 0.68052) newpose.pose.orientation = down if self.move_arm_to_pose(newpose) : rospy.loginfo("SUCCEEDED: %s" % name) return True else : rospy.logerr("FAILED %s" % name) return False def move_arm_to_pose(self, ps): arm_target_pose = deepcopy(ps) arm_target_pose.header.stamp = rospy.Time.now() self.test_pose_publisher.publish(arm_target_pose) self.arm.set_pose_target(arm_target_pose) success = self.arm.go() return success def handle_move_arm(self, req): success = True gripper = self.gripper if req.action == "OPEN_GRIPPER": rospy.loginfo("Beginning to open gripper") gripper.open(block=True) rospy.loginfo("Opened Gripper") elif req.action == "CLOSE_GRIPPER" : rospy.loginfo("Beginning to close Gripper") gripper.close(block=True) rospy.loginfo("Closed Gripper") elif req.action == "MOVE_TO_POSE_INTERMEDIATE" : rospy.loginfo("Trying to Move To Pose With Intermediate") offsets = [Vector3(0,0,.07)] success = self.MoveToPoseWithIntermediate(req.ps, offsets) elif req.action == "MOVE_TO_POSE" : rospy.loginfo("Trying to Move To Pose") success = self.MoveToPose(req.ps, "FAILED MoveToPose") elif req.action == "RESET_ARM" : rospy.loginfo("Trying to Move To Rest Pose") success = self.move_arm_to_pose(self.rest_pose) elif req.action == "PLACE_UPPER" : rospy.loginfo("Trying to Move To Place Upper Pose") success = self.move_arm_to_pose(self.place_upper_pose) # success = self.MoveToPose(self.rest_pose, "FAILED MoveToRestPose") # elif req.action == "MOVE_TO_POS" : # rospy.loginfo("Trying to Move To Pos") # new_pose = Pose() # if req.limb == 'left': # try: # self.initial_left # new_pose = deepcopy(self.initial_left) # except AttributeError: # new_pose = deepcopy(self.hand_pose_left) # self.initial_left = deepcopy(self.hand_pose_left) # elif req.limb == 'right': # try: # self.initial_right # new_pose = deepcopy(self.initial_right) # except AttributeError: # new_pose = deepcopy(self.hand_pose_right) # self.initial_right = deepcopy(self.hand_pose_right) # new_pose.position = deepcopy(req.pose.position) # # success = self.MoveToPose(req.limb, new_pose, "FAILED MoveToPose") # success = self.MoveToPoseWithIntermediate(req.limb, new_pose) # rospy.loginfo("Moved to pos: %r" % success) else : success = False rospy.logerr("invalid action") return MoveRobotResponse(success) def orientation_cb(self, data): if(data.x == -1.0 and data.y == -2.0): print "################################################" print "################### Orientation! #############" print "################################################" deg = data.z*15 rad = -deg * pi/180 + pi/2 print rad q = quaternion_from_euler(0,rad,0) self.current_pose.pose.orientation.x = q[0] self.current_pose.pose.orientation.y = q[1] self.current_pose.pose.orientation.z = q[2] self.current_pose.pose.orientation.w = q[3] else: return # transform the pose stamped to the new frame def transform_pose(self, new_frame, pose): if pose.header.frame_id == new_frame: return pose try: ps = deepcopy(pose) ps.header.stamp = rospy.Time(0) self.tfl.waitForTransform(ps.header.frame_id, new_frame, rospy.Time(0), rospy.Duration(4.0)) new_pose = self.tfl.transformPose(new_frame, ps) new_pose.header.stamp = deepcopy(pose.header.stamp) return new_pose except Exception as e: rospy.logerr(e) rospy.logerr("no transform") return None
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_cartesian_demo', anonymous=True) # 是否需要使用笛卡尔空间的运动规划 cartesian = rospy.get_param('~cartesian', True) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('manipulator') # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 arm.set_pose_reference_frame('base_link') # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.001) arm.set_goal_orientation_tolerance(0.1) # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 设置每次运动规划的时间限制:1s arm.set_planning_time(1) # 控制机械臂运动到之前设置的“home”姿态 arm.set_named_target('home') arm.go() # 获取当前位姿数据为机械臂运动的起始位姿 start_pose = arm.get_current_pose(end_effector_link).pose # --------------------- 第一段轨迹生成,关节空间插值 ---------------------# # 设置机械臂的目标位置,使用六轴的位置数据进行描述(单位:弧度) joint_positions = [0, -1, 1, 0, -1, 0] arm.set_joint_value_target(joint_positions) # 控制机械臂完成运动 arm.go() rospy.sleep(0.5) # 获取当前位姿数据为机械臂运动的起始位姿 start_pose = arm.get_current_pose(end_effector_link).pose # --------------------- 第一段轨迹生成,关节空间插值 ---------------------# # 初始化路点列表 waypoints = [] # 将初始位姿加入路点列表 if cartesian: waypoints.append(start_pose) # 设置第二个路点数据,并加入路点列表 # 第二个路点需要向后运动0.3米,向右运动0.3米 wpose = deepcopy(start_pose) wpose.orientation.x = 0 wpose.orientation.y = 0.707106781186547 wpose.orientation.z = 0 wpose.orientation.w = 0.707106781186547 wpose.position.x -= 0.2 # wpose.position.y -= 0.4 wpose.position.z -= 1 if cartesian: waypoints.append(deepcopy(wpose)) else: arm.set_pose_target(wpose) arm.go() rospy.sleep(1) # 设置第三个路点数据,并加入路点列表 wpose.orientation.x = 0 wpose.orientation.y = 0.707106781186547 wpose.orientation.z = 0 wpose.orientation.w = 0.707106781186547 # wpose.position.x -= 0.5 wpose.position.y -= 0.5 wpose.position.z += 0.2 # wpose.position.x += 0.15 # wpose.position.y += 0.1 # wpose.position.z -= 0.15 if cartesian: waypoints.append(deepcopy(wpose)) else: arm.set_pose_target(wpose) arm.go() rospy.sleep(1) # 设置第四个路点数据,回到初始位置,并加入路点列表 if cartesian: waypoints.append(deepcopy(start_pose)) else: arm.set_pose_target(start_pose) arm.go() rospy.sleep(1) if cartesian: fraction = 0.0 #路径规划覆盖率 maxtries = 100 #最大尝试规划次数 attempts = 0 #已经尝试规划次数 # 设置机器臂当前的状态作为运动初始状态 arm.set_start_state_to_current_state() # 尝试规划一条笛卡尔空间下的路径,依次通过所有路点 while fraction < 1.0 and attempts < maxtries: (plan, fraction) = arm.compute_cartesian_path ( waypoints, # waypoint poses,路点列表 0.01, # eef_step,终端步进值 0.0, # jump_threshold,跳跃阈值 True) # avoid_collisions,避障规划 # 尝试次数累加 attempts += 1 # 打印运动规划进程 if attempts % 10 == 0: rospy.loginfo("Still trying after " + str(attempts) + " attempts...") # 如果路径规划成功(覆盖率100%),则开始控制机械臂运动 if fraction == 1.0: rospy.loginfo("Path computed successfully. Moving the arm.") arm.execute(plan) rospy.loginfo("Path execution complete.") # 如果路径规划失败,则打印失败信息 else: rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.") # 控制机械臂回到初始化位置 arm.set_named_target('home') arm.go() rospy.sleep(1) # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def callback(self, data): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 创建一个存储物体颜色的字典对象 self.colors = dict() # 等待场景准备就绪 scene = PlanningSceneInterface() rospy.sleep(1) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('arm') gripper = MoveGroupCommander('gripper') # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.05) # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 reference_frame = 'base' arm.set_pose_reference_frame(reference_frame) # 设置每次运动规划的时间限制:5s arm.set_planning_time(5) # 设置场景物体的名称 table_id = 'table' # 控制机械臂先回到初始化位置 '''arm.set_named_target('home') arm.go() rospy.sleep(2)''' # 设置桌面的高度 table_ground = 0.6 # 设置table、box1和box2的三维尺寸 table_size = [1.5, 0.7, 0.01] # 将三个物体加入场景当中 table_pose = PoseStamped() table_pose.header.frame_id = reference_frame table_pose.pose.position.x = 0.0 table_pose.pose.position.y = 0.5 table_pose.pose.position.z = 0.825 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) rospy.sleep(1) # 将桌子设置成红色,两个box设置成橙色 self.setColor(table_id, 0.8, 0, 0, 1.0) # 将场景中的颜色设置发布 self.sendColors() # 设置机械臂的运动目标位置,进行避障规划 '''orient = transformations.quaternion_from_euler(0, 1.57075, 0) target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.pose.position.x = -0.1 target_pose.pose.position.y = 0.5 target_pose.pose.position.z = 1.05 target_pose.pose.orientation.x = orient[0] target_pose.pose.orientation.y = orient[1] target_pose.pose.orientation.z = orient[2] target_pose.pose.orientation.w = orient[3]''' orient = transformations.quaternion_from_euler(0, 1.57075, 0) target_pose = data target_pose.pose.orientation.x = orient[0] target_pose.pose.orientation.y = orient[1] target_pose.pose.orientation.z = orient[2] target_pose.pose.orientation.w = orient[3] # 控制机械臂运动到目标位置 self.gripper_control(0) result = None arm.set_pose_target(target_pose, end_effector_link) arm.go() rospy.loginfo('Start grasping') target_pose.pose.position.z -= 0.05 arm.set_pose_target(target_pose, end_effector_link) arm.go() self.gripper_control(0.36) '''arm.attach_object( box2_id) arm.set_named_target('home') arm.go()''' arm.set_named_target('home') arm.go() self.gripper_control(0) rospy.loginfo('Grasping done') rospy.sleep(2) judge_client(True) moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
class MoveItDemo: def __init__(self): global obj_att # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') #Initialize robot robot = moveit_commander.RobotCommander() # Use the planning scene object to add or remove objects self.scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10) # Create a publisher for displaying object frames self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10) ### Create a publisher for visualizing direction ### self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10) # Create a dictionary to hold object colors self.colors = dict() # Initialize the MoveIt! commander for the arm self.right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the MoveIt! commander for the gripper self.right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Allow 5 seconds per planning attempt self.right_arm.set_planning_time(5) # Prepare Action Controller for gripper self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction) self.ac.wait_for_server() # Give the scene a chance to catch up rospy.sleep(2) # Prepare Gazebo Subscriber self.pwh = None self.pwh_copy = None self.idx_targ = None self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback) ### OPEN THE GRIPPER ### self.open_gripper() # PREPARE THE SCENE while self.pwh is None: rospy.sleep(0.05) ############## CLEAR THE SCENE ################ # planning_scene.world.collision_objects.remove('target') # Remove leftover objects from a previous run self.scene.remove_world_object('target') self.scene.remove_world_object('table') # self.scene.remove_world_object(obstacle1_id) # Remove any attached objects from a previous session self.scene.remove_attached_object(GRIPPER_FRAME, 'target') # Run and keep in the BG the scene generator also add the ability to kill the code with ctrl^c timerThread = threading.Thread(target=self.scene_generator) timerThread.daemon = True timerThread.start() initial_pose = PoseStamped() initial_pose.header.frame_id = 'gazebo_world' initial_pose.pose = target_pose.pose print "==================== Generating Transformations ===========================" #################### PRE GRASPING POSE ######################### M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]) M1[0,3] = target_pose.pose.position.x M1[1,3] = target_pose.pose.position.y M1[2,3] = target_pose.pose.position.z M2 = transformations.euler_matrix(0, 1.57, 0) M2[0,3] = 0.0 # offset about x M2[1,3] = 0.0 # about y M2[2,3] = 0.25 # about z T = np.dot(M1, M2) pre_grasping = deepcopy(target_pose) pre_grasping.pose.position.x = T[0,3] pre_grasping.pose.position.y = T[1,3] pre_grasping.pose.position.z = T[2,3] quat = transformations.quaternion_from_matrix(T) pre_grasping.pose.orientation.x = quat[0] pre_grasping.pose.orientation.y = quat[1] pre_grasping.pose.orientation.z = quat[2] pre_grasping.pose.orientation.w = quat[3] pre_grasping.header.frame_id = 'gazebo_world' self.plan_exec(pre_grasping) #################### GRASPING POSE ######################### M3 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]) M3[0,3] = target_pose.pose.position.x M3[1,3] = target_pose.pose.position.y M3[2,3] = target_pose.pose.position.z M4 = transformations.euler_matrix(0, 1.57, 0) M4[0,3] = 0.0 # offset about x M4[1,3] = 0.0 # about y M4[2,3] = 0.18 # about z T2 = np.dot(M3, M4) grasping = deepcopy(target_pose) grasping.pose.position.x = T2[0,3] grasping.pose.position.y = T2[1,3] grasping.pose.position.z = T2[2,3] quat2 = transformations.quaternion_from_matrix(T2) grasping.pose.orientation.x = quat2[0] grasping.pose.orientation.y = quat2[1] grasping.pose.orientation.z = quat2[2] grasping.pose.orientation.w = quat2[3] grasping.header.frame_id = 'gazebo_world' self.plan_exec(grasping) #Close the gripper print "========== Waiting for gazebo to catch up ==========" self.close_gripper() #################### ATTACH OBJECT ###################### touch_links = [GRIPPER_FRAME, 'r_gripper_l_finger_tip_link','r_gripper_r_finger_tip_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link'] #print touch_links self.scene.attach_box(GRIPPER_FRAME, target_id, target_pose, target_size, touch_links) # counter to let the planning scene know when to remove the object obj_att = 1 #self.scene.remove_world_object(target_id) #################### POST-GRASP RETREAT ######################### M5 = transformations.quaternion_matrix([initial_pose.pose.orientation.x, initial_pose.pose.orientation.y, initial_pose.pose.orientation.z, initial_pose.pose.orientation.w]) M5[0,3] = initial_pose.pose.position.x M5[1,3] = initial_pose.pose.position.y M5[2,3] = initial_pose.pose.position.z M6 = transformations.euler_matrix(0, 1.57, 0) M6[0,3] = 0.0 # offset about x M6[1,3] = 0.0 # about y M6[2,3] = 0.3 # about z T3 = np.dot(M5, M6) post_grasping = deepcopy(initial_pose) post_grasping.pose.position.x = T3[0,3] post_grasping.pose.position.y = T3[1,3] post_grasping.pose.position.z = T3[2,3] quat3 = transformations.quaternion_from_matrix(T3) post_grasping.pose.orientation.x = quat3[0] post_grasping.pose.orientation.y = quat3[1] post_grasping.pose.orientation.z = quat3[2] post_grasping.pose.orientation.w = quat3[3] post_grasping.header.frame_id = 'gazebo_world' self.plan_exec(post_grasping) # Specify a pose to place the target after being picked up place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = 0.52 place_pose.pose.position.y = -0.48 place_pose.pose.position.z = 0.48 place_pose.pose.orientation.w = 1.0 n_attempts = 0 max_place_attempts = 2 # Generate valid place poses places = self.make_places(place_pose) success = False # Repeat until we succeed or run out of attempts while success == False and n_attempts < max_place_attempts: for place in places: success = self.right_arm.place(target_id, place) if success: break n_attempts += 1 rospy.loginfo("Place attempt: " + str(n_attempts)) rospy.sleep(0.2) self.open_gripper() obj_att = None rospy.sleep(3) ## # Initialize the grasp object ## g = Grasp() ## grasps = [] ## # Set the first grasp pose to the input pose ## g.grasp_pose = pre_grasping ## g.allowed_touch_objects = [target_id] ## grasps.append(deepcopy(g)) ## right_arm.pick(target_id, grasps) # #Change the frame_id for the planning to take place! # #target_pose.header.frame_id = 'gazebo_world' # # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # # Exit the script moveit_commander.os._exit(0) ################################################################################################################## #Get pose from Gazebo def model_state_callback(self,msg): self.pwh = ModelStates() self.pwh = msg # Generate a list of possible place poses def make_places(self, init_pose): # Initialize the place location as a PoseStamped message place = PoseStamped() # Start with the input place pose place = init_pose # A list of x shifts (meters) to try x_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015] # A list of y shifts (meters) to try y_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015] # A list of pitch angles to try #pitch_vals = [0, 0.005, -0.005, 0.01, -0.01, 0.02, -0.02] pitch_vals = [0] # A list of yaw angles to try yaw_vals = [0] # A list to hold the places places = [] # Generate a place pose for each angle and translation for y in yaw_vals: for p in pitch_vals: for y in y_vals: for x in x_vals: place.pose.position.x = init_pose.pose.position.x + x place.pose.position.y = init_pose.pose.position.y + y # Create a quaternion from the Euler angles q = quaternion_from_euler(0, p, y) # Set the place pose orientation accordingly place.pose.orientation.x = q[0] place.pose.orientation.y = q[1] place.pose.orientation.z = q[2] place.pose.orientation.w = q[3] # Append this place pose to the list places.append(deepcopy(place)) # Return the list return places def plan_exec(self, pose): self.right_arm.clear_pose_targets() self.right_arm.set_pose_target(pose, GRIPPER_FRAME) self.right_arm.plan() rospy.sleep(5) self.right_arm.go(wait=True) def close_gripper(self): g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.035, 100)) self.ac.send_goal(g_close) self.ac.wait_for_result() rospy.sleep(15) # Gazebo requires up to 15 seconds to attach object def open_gripper(self): g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100)) self.ac.send_goal(g_open) self.ac.wait_for_result() rospy.sleep(5) # And up to 20 to detach it def scene_generator(self): # print obj_att global target_pose global target_id global target_size target_id = 'target' self.taid = self.pwh.name.index('wood_cube_5cm') table_id = 'table' self.tid = self.pwh.name.index('table') #obstacle1_id = 'obstacle1' #self.o1id = self.pwh.name.index('wood_block_10_2_1cm') # Set the target size [l, w, h] target_size = [0.05, 0.05, 0.05] table_size = [1.5, 0.8, 0.03] #obstacle1_size = [0.1, 0.025, 0.01] ## Set the target pose on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose = self.pwh.pose[self.taid] target_pose.pose.position.z += 0.025 # Add the target object to the scene if obj_att is None: self.scene.add_box(target_id, target_pose, target_size) table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose = self.pwh.pose[self.tid] table_pose.pose.position.z += 1 self.scene.add_box(table_id, table_pose, table_size) #obstacle1_pose = PoseStamped() #obstacle1_pose.header.frame_id = REFERENCE_FRAME #obstacle1_pose.pose = self.pwh.pose[self.o1id] ## Add the target object to the scene #scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size) # Specify a pose to place the target after being picked up place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = 0.50 place_pose.pose.position.y = -0.30 place_pose.pose.orientation.w = 1.0 # Add the target object to the scene self.scene.add_box(target_id, target_pose, target_size) ### Make the target purple ### self.setColor(target_id, 0.6, 0, 1, 1.0) # Send the colors to the planning scene self.sendColors() else: self.scene.remove_world_object('target') # Publish targe's frame #self.object_frames_pub.publish(target_pose) threading.Timer(0.5, self.scene_generator).start() # Set the color of an object def setColor(self, name, r, g, b, a = 0.9): # Initialize a MoveIt color object color = ObjectColor() # Set the id to the name given as an argument color.id = name # Set the rgb and alpha values given as input color.color.r = r color.color.g = g color.color.b = b color.color.a = a # Update the global color dictionary self.colors[name] = color # Actually send the colors to MoveIt! def sendColors(self): # Initialize a planning scene object p = PlanningScene() # Need to publish a planning scene diff p.is_diff = True # Append the colors from the global color dictionary for color in self.colors.values(): p.object_colors.append(color) # Publish the scene diff self.scene_pub.publish(p)
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_obstacles_demo') # 初始化场景对象 scene = PlanningSceneInterface() # 创建一个发布场景变化信息的发布者 self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) # 创建一个存储物体颜色的字典对象 self.colors = dict() # 等待场景准备就绪 rospy.sleep(1) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('ur_arm') # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.05) # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) # 设置每次运动规划的时间限制:5s arm.set_planning_time(5) # 设置场景物体的名称 table_id = 'table' box1_id = 'box1' box2_id = 'box2' # 移除场景中之前运行残留的物体 scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) rospy.sleep(1) # 控制机械臂先回到初始化位置 arm.set_named_target('home') arm.go() rospy.sleep(2) # 设置桌面的高度 table_ground = 0.25 # 设置table、box1和box2的三维尺寸 table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # 将三个物体加入场景当中 table_pose = PoseStamped() table_pose.header.frame_id = reference_frame table_pose.pose.position.x = 0.26 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose = PoseStamped() box1_pose.header.frame_id = reference_frame box1_pose.pose.position.x = 0.21 box1_pose.pose.position.y = -0.1 box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0 box1_pose.pose.orientation.w = 1.0 scene.add_box(box1_id, box1_pose, box1_size) box2_pose = PoseStamped() box2_pose.header.frame_id = reference_frame box2_pose.pose.position.x = 0.19 box2_pose.pose.position.y = 0.15 box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 scene.add_box(box2_id, box2_pose, box2_size) # 将桌子设置成红色,两个box设置成橙色 self.setColor(table_id, 0.8, 0, 0, 1.0) self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # 将场景中的颜色设置发布 self.sendColors() # 设置机械臂的运动目标位置,位于桌面之上两个盒子之间 target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.pose.position.x = 0.2 target_pose.pose.position.y = 0.0 target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05 target_pose.pose.orientation.w = 1.0 # 控制机械臂运动到目标位置 arm.set_pose_target(target_pose, end_effector_link) arm.go() rospy.sleep(2) # 设置机械臂的运动目标位置,进行避障规划 target_pose2 = PoseStamped() target_pose2.header.frame_id = reference_frame target_pose2.pose.position.x = 0.2 target_pose2.pose.position.y = -0.25 target_pose2.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05 target_pose2.pose.orientation.w = 1.0 # 控制机械臂运动到目标位置 arm.set_pose_target(target_pose2, end_effector_link) arm.go() rospy.sleep(2) # 控制机械臂回到初始化位置 arm.set_named_target('home') arm.go() # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') #Initialize robot robot = moveit_commander.RobotCommander() # Use the planning scene object to add or remove objects scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10) # Create a publisher for displaying object frames self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10) # Create a dictionary to hold object colors self.colors = dict() # Initialize the MoveIt! commander for the arm right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the MoveIt! commander for the gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link eef = right_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) # right_arm.set_goal_position_tolerance(0.05) # right_arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the right arm reference frame right_arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt right_arm.set_planning_time(5) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 5 # Set a limit on the number of place attempts max_place_attempts = 5 # Give the scene a chance to catch up rospy.sleep(2) # Prepare Gazebo Subscriber self.pwh = None self.pwh_copy = None self.idx_targ = None self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback) # Prepare Gripper and open it self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction) self.ac.wait_for_server() g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100)) g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.0, 1)) self.ac.send_goal(g_open) rospy.sleep(2) # PREPARE THE SCENE while self.pwh is None: rospy.sleep(0.05) target_id = 'target' self.taid = self.pwh.name.index('wood_cube_5cm') table_id = 'table' self.tid = self.pwh.name.index('table') #obstacle1_id = 'obstacle1' #self.o1id = self.pwh.name.index('wood_block_10_2_1cm') # Remove leftover objects from a previous run scene.remove_world_object(target_id) scene.remove_world_object(table_id) #scene.remove_world_object(obstacle1_id) # Remove any attached objects from a previous session scene.remove_attached_object(GRIPPER_FRAME, target_id) # Set the target size [l, w, h] target_size = [0.05, 0.05, 0.05] table_size = [1.5, 0.8, 0.03] #obstacle1_size = [0.1, 0.025, 0.01] ## Set the target pose on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose = self.pwh.pose[self.taid] target_pose.pose.position.z += 0.025 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose = self.pwh.pose[self.tid] table_pose.pose.position.z += 1 scene.add_box(table_id, table_pose, table_size) #obstacle1_pose = PoseStamped() #obstacle1_pose.header.frame_id = REFERENCE_FRAME #obstacle1_pose.pose = self.pwh.pose[self.o1id] ## Add the target object to the scene #scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size) # Specify a pose to place the target after being picked up place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = 0.50 place_pose.pose.position.y = -0.30 place_pose.pose.orientation.w = 1.0 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) ### Make the target purple ### self.setColor(target_id, 0.6, 0, 1, 1.0) # Send the colors to the planning scene self.sendColors() #print target_pose self.object_frames_pub.publish(target_pose) rospy.sleep(2) # Initialize the grasp pose to the target pose grasp_pose = target_pose #grasp_pose.header.frame_id = 'gazebo_wolrd' # Shift the grasp pose by half the width of the target to center it # grasp_pose.pose.position.y -= target_size[1] / 2.0 # grasp_pose.pose.position.x = target_pose.pose.position.x / 2.0 # grasp_pose.pose.position.x = target_pose.pose.position.x -0.07 # grasp_pose.pose.position.z += 0.18 #Allowed touch object list # ato = [target_id, 'r_forearm_link'] # Generate a list of grasps grasps = self.make_grasps(grasp_pose, [target_id]) #### [target_id] # Publish the grasp poses so they can be viewed in RViz for grasp in grasps: # print grasp.grasp_pose self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) # Track success/failure and number of attempts for pick operation success = False n_attempts = 0 #Allowed touch link list atl = ['r_forearm_link'] # Repeat until we succeed or run out of attempts while success == False and n_attempts < max_pick_attempts: success = right_arm.pick(target_id, grasps) n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) rospy.sleep(0.2) if success: self.ac.send_goal(g_close) rospy.sleep(3) ## If the pick was successful, attempt the place operation #if success: #success = False #n_attempts = 0 ## Generate valid place poses #places = self.make_places(place_pose) ## Repeat until we succeed or run out of attempts #while success == False and n_attempts < max_place_attempts: #for place in places: #success = right_arm.place(target_id, place) #if success: #break #n_attempts += 1 #rospy.loginfo("Place attempt: " + str(n_attempts)) #rospy.sleep(0.2) #if not success: #rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.") #else: #rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.") ## Return the arm to the "resting" pose stored in the SRDF file ##right_arm.set_named_target('resting') ##right_arm.go() ## Open the gripper to the neutral position #right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) #right_gripper.go() #rospy.sleep(1) #rospy.spin() # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) rospy.Subscriber('camera/depth_registered/points', PointCloud2, point_callback) rospy.Subscriber('darknet_ros/bounding_boxes', BoundingBoxes, bbox_callback) display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) self.ii = 0 global goal_x global goal_y global goal_z # self.listener = tf.TransformListener() #self.goal_x = 0 #self.goal_y = 0 #self.goal_z = 0 self.goal_ori_x = 0 self.goal_ori_y = 0 self.goal_ori_z = 0 self.goal_ori_w = 0 # self.marker = [] self.tomato = [] self.position_list = [] self.orientation_list = [] self.t_idd = 0 self.t_pose_x = [] self.t_pose_y = [] self.t_pose_z = [] self.t_ori_w = [] self.t_ori_x = [] self.t_ori_y = [] self.t_ori_z = [] self.tomato_pose = Pose() self.goalPoseFromTomato = Pose() self.br = tf.TransformBroadcaster() self.calculated_tomato = Pose() self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty) self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(5) self.robot_arm.set_num_planning_attempts(5) self.display_trajectory_publisher = display_trajectory_publisher rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) self.clear_octomap() def move_code(self): planning_frame = self.robot_arm.get_planning_frame() print("====== planning frame: ", planning_frame) self.wpose = self.robot_arm.get_current_pose() print("====== current pose : ", self.wpose) joint_goal = [ -0.535054565144069, -2.009213503260451, 1.8350906250920112, -0.7794355413099039, -0.7980899690645948, 0.7782740454087982 ] print("INIT POSE: ", self.robot_arm.get_current_pose().pose.position) self.robot_arm.go(joint_goal, wait=True) def go_to_move(self, scale=1.0): planning_frame = self.robot_arm.get_planning_frame() tomato_offset = [0, -0.35, -0.1] robot_base_offset = 0.873 base_wrist2_offset = 0.1 print(">> robot arm planning frame: \n", planning_frame) ''' self.calculated_tomato.position.x = (scale*self.goal_x) self.calculated_tomato.position.y = (scale*self.goal_y) self.calculated_tomato.position.z = (scale*self.goal_z) ''' self.calculated_tomato.position.x = (scale * goal_x) self.calculated_tomato.position.y = (scale * goal_y) self.calculated_tomato.position.z = (scale * goal_z) self.calculated_tomato.orientation = Quaternion( *quaternion_from_euler(3.14, 0.1, 1.57)) print("=== robot_pose goal frame: ", self.calculated_tomato) self.robot_arm.set_pose_target(self.calculated_tomato) tf_display_position = [ self.calculated_tomato.position.x, self.calculated_tomato.position.y, self.calculated_tomato.position.z ] tf_display_orientation = [ self.calculated_tomato.orientation.x, self.calculated_tomato.orientation.y, self.calculated_tomato.orientation.z, self.calculated_tomato.orientation.w ] ii = 0 while ii < 5: ii += 1 self.br.sendTransform(tf_display_position, tf_display_orientation, rospy.Time.now(), "goal_wpose", "world") rate.sleep() tomato_waypoints = [] tomato_waypoints.append(copy.deepcopy(self.calculated_tomato)) (tomato_plan, tomato_offset) = self.robot_arm.compute_cartesian_path( tomato_waypoints, 0.01, 0.0) self.display_trajectory(tomato_plan) print("=== Press `Enter` to if plan is correct!! ...") raw_input() self.robot_arm.go(True) def go_to_desired_coordinate(self, pose_x, pose_y, pose_z, roll, pitch, yaw): ''' Manipulator is moving to the desired coordinate Now move to the calculated_tomato_id_goal ''' calculated_tomato_id_goal = Pose() desired_goal_pose = [pose_x, pose_y, pose_z] desired_goal_euler = [roll, pitch, yaw] Cplanning_frame = self.robot_arm.get_planning_frame() print(">> current planning frame: \n", Cplanning_frame) calculated_tomato_id_goal.position.x = desired_goal_pose[0] calculated_tomato_id_goal.position.y = desired_goal_pose[1] calculated_tomato_id_goal.position.z = desired_goal_pose[2] calculated_tomato_id_goal.orientation = Quaternion( *quaternion_from_euler(desired_goal_euler[0], desired_goal_euler[1], desired_goal_euler[2])) print(">> tomato_id_goal frame: ", desired_goal_pose) self.robot_arm.set_pose_target(calculated_tomato_id_goal) tf_display_position = [ calculated_tomato_id_goal.position.x, calculated_tomato_id_goal.position.x, calculated_tomato_id_goal.position.z ] tf_display_orientation = [ calculated_tomato_id_goal.orientation.x, calculated_tomato_id_goal.orientation.y, calculated_tomato_id_goal.orientation.z, calculated_tomato_id_goal.orientation.w ] ii = 0 while ii < 5: ii += 1 self.br.sendTransform(tf_display_position, tf_display_orientation, rospy.Time.now(), "goal_wpose", "world") rate.sleep() ## ## ## show how to move on the Rviz tomato_id_goal_waypoints = [] tomato_id_goal_waypoints.append( copy.deepcopy(calculated_tomato_id_goal)) (tomato_id_goal_plan, tomato_id_goal_fraction) = self.robot_arm.compute_cartesian_path( tomato_id_goal_waypoints, 0.01, 0.0) self.display_trajectory(tomato_id_goal_plan) print("============ Press `Enter` to if plan is correct!! ...") raw_input() self.robot_arm.go(True) def display_trajectory(self, plan): display_trajectory_publisher = self.display_trajectory_publisher display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = self.robot_cmd.get_current_state( ) display_trajectory.trajectory.append(plan) display_trajectory_publisher.publish(display_trajectory) def plan_cartesian_path(self, x_offset, y_offset, z_offset, scale=1.0): waypoints = [] ii = 1 self.wpose = self.robot_arm.get_current_pose().pose self.wpose.position.x = (scale * self.wpose.position.x) + x_offset # - 0.10 waypoints.append(copy.deepcopy(self.wpose)) ii += 1 self.wpose.position.y = (scale * self.wpose.position.y) + y_offset # + 0.05 waypoints.append(copy.deepcopy(self.wpose)) ii += 1 self.wpose.position.z = (scale * self.wpose.position.z) + z_offset waypoints.append(copy.deepcopy(self.wpose)) ii += 1 (plan, fraction) = self.robot_arm.compute_cartesian_path( waypoints, 0.01, 0.0) return plan, fraction def plan_cartesian_x(self, x_offset, scale=1.0): waypoints = [] self.wpose = self.robot_arm.get_current_pose().pose self.wpose.position.x = (scale * self.wpose.position.x) + x_offset # -0.10 waypoints.append(copy.deepcopy(self.wpose)) (plan, fraction) = self.robot_arm.compute_cartesian_path( waypoints, 0.01, 0.0) return plan, fraction def plan_cartesian_y(self, y_offset, scale=1.0): waypoints = [] self.wpose = self.robot_arm.get_current_pose().pose self.wpose.position.y = (scale * self.wpose.position.y) + y_offset # -0.10 waypoints.append(copy.deepcopy(self.wpose)) (plan, fraction) = self.robot_arm.compute_cartesian_path( waypoints, 0.01, 0.0) return plan, fraction def plan_cartesian_z(self, z_offset, scale=1.0): waypoints = [] self.wpose = self.robot_arm.get_current_pose().pose self.wpose.position.z = (scale * self.wpose.position.z) + z_offset # -0.10 waypoints.append(copy.deepcopy(self.wpose)) (plan, fraction) = self.robot_arm.compute_cartesian_path( waypoints, 0.01, 0.0) return plan, fraction def execute_plan(self, plan): group = self.robot_arm group.execute(plan, wait=True) def pose_subscriber(self): rospy.loginfo("waiting for pose topic...") rospy.Subscriber('camera/depth_registered/points', PointCloud2, point_callback) rospy.Subscriber('darknet_ros/bounding_boxes', BoundingBoxes, bbox_callback)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) # Initialize the ROS node rospy.init_node('moveit_demo', anonymous=True) robot = RobotCommander() # Connect to the right_arm move group right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Increase the planning time since contraint planning can take a while right_arm.set_planning_time(15) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the right arm reference frame right_arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow some leeway in position(meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.05) right_arm.set_goal_orientation_tolerance(0.1) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Start in the "resting" configuration stored in the SRDF file right_arm.set_named_target('resting') # Plan and execute a trajectory to the goal configuration right_arm.go() rospy.sleep(1) # Open the gripper right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Set an initial target pose with the arm up and to the right target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.237012590198 target_pose.pose.position.y = -0.0747191267505 target_pose.pose.position.z = 0.901578401949 target_pose.pose.orientation.w = 1.0 # Set the start state and target pose, then plan and execute right_arm.set_start_state(robot.get_current_state()) right_arm.set_pose_target(target_pose, end_effector_link) right_arm.go() rospy.sleep(2) # Close the gripper right_gripper.set_joint_value_target(GRIPPER_CLOSED) right_gripper.go() rospy.sleep(1) # Store the current pose start_pose = right_arm.get_current_pose(end_effector_link) # Create a contraints list and give it a name constraints = Constraints() constraints.name = "Keep gripper horizontal" # Create an orientation constraint for the right gripper orientation_constraint = OrientationConstraint() orientation_constraint.header = start_pose.header orientation_constraint.link_name = right_arm.get_end_effector_link() orientation_constraint.orientation.w = 1.0 orientation_constraint.absolute_x_axis_tolerance = 0.1 orientation_constraint.absolute_y_axis_tolerance = 0.1 orientation_constraint.absolute_z_axis_tolerance = 3.14 orientation_constraint.weight = 1.0 # Append the constraint to the list of contraints constraints.orientation_constraints.append(orientation_constraint) # Set the path constraints on the right_arm right_arm.set_path_constraints(constraints) # Set a target pose for the arm target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.173187824708 target_pose.pose.position.y = -0.0159929871606 target_pose.pose.position.z = 0.692596608605 target_pose.pose.orientation.w = 1.0 # Set the start state and target pose, then plan and execute right_arm.set_start_state_to_current_state() right_arm.set_pose_target(target_pose, end_effector_link) right_arm.go() rospy.sleep(1) # Clear all path constraints right_arm.clear_path_constraints() # Open the gripper right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Return to the "resting" configuration stored in the SRDF file right_arm.set_named_target('resting') # Plan and execute a trajectory to the goal configuration right_arm.go() rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit MoveIt moveit_commander.os._exit(0)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) # Initialize the ROS node rospy.init_node('moveit_constraints_demo', anonymous=True) robot = RobotCommander() # Connect to the arm move group arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Increase the planning time since constraint planning can take a while arm.set_planning_time(5) # Allow replanning to increase the odds of a solution arm.allow_replanning(True) # Set the right arm reference frame arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow some leeway in position(meters) and orientation (radians) arm.set_goal_position_tolerance(0.05) arm.set_goal_orientation_tolerance(0.1) # Get the name of the end-effector link end_effector_link = arm.get_end_effector_link() # Start in the "resting" configuration stored in the SRDF file arm.set_named_target('l_arm_init') # Plan and execute a trajectory to the goal configuration arm.go() rospy.sleep(1) # Open the gripper gripper.set_joint_value_target(GRIPPER_NEUTRAL) gripper.go() rospy.sleep(1) # Set an initial target pose with the arm up and to the right target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.263803774718 target_pose.pose.position.y = 0.295405791959 target_pose.pose.position.z = 0.690438884208 q = quaternion_from_euler(0, 0, -1.57079633) target_pose.pose.orientation.x = q[0] target_pose.pose.orientation.y = q[1] target_pose.pose.orientation.z = q[2] target_pose.pose.orientation.w = q[3] # Set the start state and target pose, then plan and execute arm.set_start_state(robot.get_current_state()) arm.set_pose_target(target_pose, end_effector_link) arm.go() rospy.sleep(2) # Close the gripper gripper.set_joint_value_target(GRIPPER_CLOSED) gripper.go() rospy.sleep(1) # Store the current pose start_pose = arm.get_current_pose(end_effector_link) # Create a contraints list and give it a name constraints = Constraints() constraints.name = "Keep gripper horizontal" # Create an orientation constraint for the right gripper orientation_constraint = OrientationConstraint() orientation_constraint.header = start_pose.header orientation_constraint.link_name = arm.get_end_effector_link() orientation_constraint.orientation.w = 1.0 orientation_constraint.absolute_x_axis_tolerance = 0.1 orientation_constraint.absolute_y_axis_tolerance = 0.1 orientation_constraint.absolute_z_axis_tolerance = 0.1 orientation_constraint.weight = 1.0 # q = quaternion_from_euler(0, 0, -1.57079633) # orientation_constraint.orientation.x = q[0] # orientation_constraint.orientation.y = q[1] # orientation_constraint.orientation.z = q[2] # orientation_constraint.orientation.w = q[3] # Append the constraint to the list of contraints constraints.orientation_constraints.append(orientation_constraint) # Set the path constraints on the arm arm.set_path_constraints(constraints) # Set a target pose for the arm target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.39000848183 target_pose.pose.position.y = 0.185900663329 target_pose.pose.position.z = 0.732752341378 target_pose.pose.orientation.w = 1 # Set the start state and target pose, then plan and execute arm.set_start_state_to_current_state() arm.set_pose_target(target_pose, end_effector_link) arm.go() rospy.sleep(1) # Clear all path constraints arm.clear_path_constraints() # Open the gripper gripper.set_joint_value_target(GRIPPER_NEUTRAL) gripper.go() rospy.sleep(1) # Return to the "resting" configuration stored in the SRDF file arm.set_named_target('l_arm_init') # Plan and execute a trajectory to the goal configuration arm.go() rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit MoveIt moveit_commander.os._exit(0)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Construct the initial scene object scene = PlanningSceneInterface() # Pause for the scene to get ready rospy.sleep(1) # Initialize the MoveIt! commander for the right arm right_arm = MoveGroupCommander('right_arm') # Initialize the MoveIt! commander for the gripper right_gripper = MoveGroupCommander('right_gripper') # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.01) right_arm.set_goal_orientation_tolerance(0.05) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Allow 5 seconds per planning attempt right_arm.set_planning_time(5) # Remove leftover objects from a previous run scene.remove_attached_object(end_effector_link, 'tool') scene.remove_world_object('table') scene.remove_world_object('box1') scene.remove_world_object('box2') scene.remove_world_object('target') # Set the height of the table off the ground table_ground = 0.75 # Set the length, width and height of the table table_size = [0.2, 0.7, 0.01] # Set the length, width and height of the object to attach tool_size = [0.3, 0.02, 0.02] # Create a pose for the tool relative to the end-effector p = PoseStamped() p.header.frame_id = end_effector_link scene.attach_mesh # Place the end of the object within the grasp of the gripper p.pose.position.x = tool_size[0] / 2.0 - 0.025 p.pose.position.y = 0.0 p.pose.position.z = 0.0 # Align the object with the gripper (straight out) p.pose.orientation.x = 0 p.pose.orientation.y = 0 p.pose.orientation.z = 0 p.pose.orientation.w = 1 # Attach the tool to the end-effector scene.attach_box(end_effector_link, 'tool', p, tool_size) # Add a floating table top table_pose = PoseStamped() table_pose.header.frame_id = 'base_footprint' table_pose.pose.position.x = 0.35 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box('table', table_pose, table_size) # Update the current state right_arm.set_start_state_to_current_state() # Move the arm with the attached object to the 'straight_forward' position right_arm.set_named_target('straight_forward') right_arm.go() rospy.sleep(2) # Return the arm in the "resting" pose stored in the SRDF file right_arm.set_named_target('resting') right_arm.go() rospy.sleep(2) scene.remove_attached_object(end_effector_link, 'tool') moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Use the planning scene object to add or remove objects scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=5) # Create a dictionary to hold object colors self.colors = dict() # Initialize the move group for the arm arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the gripper gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) arm.set_goal_position_tolerance(0.05) arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution arm.allow_replanning(True) # Set the right arm reference frame arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt arm.set_planning_time(5) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 5 # Set a limit on the number of place attempts max_place_attempts = 5 # Give the scene a chance to catch up rospy.sleep(2) # Give each of the scene objects a unique name table_id = 'table' box1_id = 'box1' box2_id = 'box2' target_id = 'target' tool_id = 'tool' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_world_object(target_id) scene.remove_world_object(tool_id) # Remove any attached objects from a previous session scene.remove_attached_object(GRIPPER_FRAME, target_id) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "grasp" pose stored in the SRDF file arm.set_named_target('left_arm_up') arm.go() # Open the gripper to the neutral position gripper.set_joint_value_target(GRIPPER_OPEN) gripper.go() rospy.sleep(1) # Set the height of the table off the ground table_ground = 0.04 # Set the dimensions of the scene objects [l, w, h] table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # Set the target size [l, w, h] target_size = [0.02, 0.01, 0.12] # Add a table top and two boxes to the scene table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose.position.x = 0.25 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose = PoseStamped() box1_pose.header.frame_id = REFERENCE_FRAME box1_pose.pose.position.x = 0.21 box1_pose.pose.position.y = -0.1 box1_pose.pose.position.z = table_ground + table_size[ 2] + box1_size[2] / 2.0 box1_pose.pose.orientation.w = 1.0 #scene.add_box(box1_id, box1_pose, box1_size) box2_pose = PoseStamped() box2_pose.header.frame_id = REFERENCE_FRAME box2_pose.pose.position.x = 0.19 box2_pose.pose.position.y = 0.13 box2_pose.pose.position.z = table_ground + table_size[ 2] + box2_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 #scene.add_box(box2_id, box2_pose, box2_size) # Set the target pose in between the boxes and on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.24 target_pose.pose.position.y = 0.275 target_pose.pose.position.z = table_ground + table_size[ 2] + target_size[2] / 2.0 target_pose.pose.orientation.w = 1.0 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) # Make the table blue and the boxes orange self.setColor(table_id, 0, 0, 0.8, 1.0) self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # Make the target yellow self.setColor(target_id, 0.9, 0.9, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the support surface name to the table object arm.set_support_surface_name(table_id) # Specify a pose to place the target after being picked up place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = 0.18 place_pose.pose.position.y = 0 place_pose.pose.position.z = table_ground + table_size[ 2] + target_size[2] / 2.0 place_pose.pose.orientation.w = 1.0 # Initialize the grasp pose to the target pose grasp_pose = target_pose # Shift the grasp pose by half the width of the target to center it #grasp_pose.pose.position.y -= target_size[1] / 2.0 grasp_pose.pose.position.x = 0.12792118579 + .1 grasp_pose.pose.position.y = 0.285290879999 + 0.05 grasp_pose.pose.position.z = 0.120301181892 #grasp_pose.pose.orientation = # Generate a list of grasps grasps = self.make_grasps(grasp_pose, [target_id, table_id]) # Publish the grasp poses so they can be viewed in RViz for grasp in grasps: self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) break # Track success/failure and number of attempts for pick operation result = None n_attempts = 0 # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) result = arm.pick(target_id, grasps) rospy.sleep(0.2) # If the pick was successful, attempt the place operation if result == MoveItErrorCodes.SUCCESS: result = None n_attempts = 0 # Generate valid place poses places = self.make_places(place_pose) # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts: n_attempts += 1 rospy.loginfo("Place attempt: " + str(n_attempts)) for place in places: result = arm.place(target_id, place) if result == MoveItErrorCodes.SUCCESS: break rospy.sleep(0.2) if result != MoveItErrorCodes.SUCCESS: rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.") else: # Return the arm to the "resting" pose stored in the SRDF file arm.set_named_target('left_arm_rest') arm.go() # Open the gripper to the open position gripper.set_joint_value_target(GRIPPER_OPEN) gripper.go() else: rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.") rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
class MoveItDemo: def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') #Initialize robot robot = moveit_commander.RobotCommander() # Use the planning scene object to add or remove objects self.scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10) # Create a publisher for displaying object frames self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10) ### Create a publisher for visualizing direction ### self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10) # Create a dictionary to hold object colors self.colors = dict() # Initialize the MoveIt! commander for the arm self.right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the MoveIt! commander for the gripper self.right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Allow 5 seconds per planning attempt self.right_arm.set_planning_time(5) # Prepare Action Controller for gripper self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction) self.ac.wait_for_server() # Give the scene a chance to catch up rospy.sleep(2) # Prepare Gazebo Subscriber self.pwh = None self.pwh_copy = None self.idx_targ = None self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback) ### OPEN THE GRIPPER ### self.open_gripper() self.obj_att = None # PREPARE THE SCENE while self.pwh is None: rospy.sleep(0.05) ############## CLEAR THE SCENE ################ # planning_scene.world.collision_objects.remove('target') # Remove leftover objects from a previous run self.scene.remove_world_object('target') self.scene.remove_world_object('table') # self.scene.remove_world_object(obstacle1_id) # Remove any attached objects from a previous session self.scene.remove_attached_object(GRIPPER_FRAME, 'target') # Run and keep in the BG the scene generator also add the ability to kill the code with ctrl^c timerThread = threading.Thread(target=self.scene_generator) timerThread.daemon = True timerThread.start() initial_pose = target_pose initial_pose.header.frame_id = 'gazebo_world' print "==================== Generating Transformations ===========================" #################### PRE GRASPING POSE ######################### # M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]) # M1[0,3] = target_pose.pose.position.x # M1[1,3] = target_pose.pose.position.y # M1[2,3] = target_pose.pose.position.z # M2 = transformations.euler_matrix(0, 1.57, 0) # M2[0,3] = 0.0 # offset about x # M2[1,3] = 0.0 # about y # M2[2,3] = 0.25 # about z # T = np.dot(M1, M2) # pre_grasping = deepcopy(target_pose) # pre_grasping.pose.position.x = T[0,3] # pre_grasping.pose.position.y = T[1,3] # pre_grasping.pose.position.z = T[2,3] # quat = transformations.quaternion_from_matrix(T) # pre_grasping.pose.orientation.x = quat[0] # pre_grasping.pose.orientation.y = quat[1] # pre_grasping.pose.orientation.z = quat[2] # pre_grasping.pose.orientation.w = quat[3] # pre_grasping.header.frame_id = 'gazebo_world' # self.plan_exec(pre_grasping) ################# GENERATE GRASPS ################### # Set a limit on the number of pick attempts before bailing max_pick_attempts = 5 # Track success/failure and number of attempts for pick operation success = False n_attempts = 0 grasps = self.grasp_generator(initial_pose) possible_grasps = [] for grasp in grasps: self.gripper_pose_pub.publish(grasp) possible_grasps.append(grasp.pose) rospy.sleep(0.2) #print possible_grasps self.right_arm.pick(target_id, grasps) # target_name = target_id # group_name = GROUP_NAME_ARM # end_effector = GROUP_NAME_GRIPPER # attached_object_touch_links = ['r_forearm_link'] # #print (target_name, group_name, end_effector) # PickupGoal(target_name, group_name ,end_effector, possible_grasps ) # # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # # Exit the script moveit_commander.os._exit(0) ################################################################# FUNCTIONS ################################################################################# def model_state_callback(self,msg): self.pwh = ModelStates() self.pwh = msg def grasp_generator(self, initial_pose): # Pitch angles to try pitch_vals = [1, 1.57,0, 1,2 ] # Yaw angles to try yaw_vals = [0]#, 1.57, -1.57] # A list to hold the grasps grasps = [] g = PoseStamped() g.header.frame_id = REFERENCE_FRAME g.pose = initial_pose.pose #g.pose.position.z += 0.18 # Generate a grasp for each pitch and yaw angle for y in yaw_vals: for p in pitch_vals: # Create a quaternion from the Euler angles q = transformations.quaternion_from_euler(0, p, y) # Set the grasp pose orientation accordingly g.pose.orientation.x = q[0] g.pose.orientation.y = q[1] g.pose.orientation.z = q[2] g.pose.orientation.w = q[3] # Append the grasp to the list grasps.append(deepcopy(g)) # Return the list return grasps def plan_exec(self, pose): self.right_arm.clear_pose_targets() self.right_arm.set_pose_target(pose, GRIPPER_FRAME) self.right_arm.plan() rospy.sleep(5) self.right_arm.go(wait=True) def close_gripper(self): g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.035, 100)) self.ac.send_goal(g_close) self.ac.wait_for_result() rospy.sleep(15) # Gazebo requires up to 15 seconds to attach object def open_gripper(self): g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100)) self.ac.send_goal(g_open) self.ac.wait_for_result() rospy.sleep(5) # And up to 20 to detach it def scene_generator(self): # print obj_att global target_pose global target_id next_call = time.time() while True: next_call = next_call+1 target_id = 'target' self.taid = self.pwh.name.index('wood_cube_5cm') table_id = 'table' self.tid = self.pwh.name.index('table') # obstacle1_id = 'obstacle1' # self.o1id = self.pwh.name.index('wood_block_10_2_1cm') # Set the target size [l, w, h] target_size = [0.05, 0.05, 0.05] table_size = [1.5, 0.8, 0.03] # obstacle1_size = [0.1, 0.025, 0.01] ## Set the target pose on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose = self.pwh.pose[self.taid] target_pose.pose.position.z += 0.025 if self.obj_att is None: # Add the target object to the scene self.scene.add_box(target_id, target_pose, target_size) table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose = self.pwh.pose[self.tid] table_pose.pose.position.z += 1 self.scene.add_box(table_id, table_pose, table_size) # obstacle1_pose = PoseStamped() # obstacle1_pose.header.frame_id = REFERENCE_FRAME # obstacle1_pose.pose = self.pwh.pose[self.o1id] # # Add the target object to the scene # scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size) ### Make the target purple ### self.setColor(target_id, 0.6, 0, 1, 1.0) # Send the colors to the planning scene self.sendColors() else: self.scene.remove_world_object('target') time.sleep(next_call - time.time()) #threading.Timer(0.5, self.scene_generator).start() # Set the color of an object def setColor(self, name, r, g, b, a = 0.9): # Initialize a MoveIt color object color = ObjectColor() # Set the id to the name given as an argument color.id = name # Set the rgb and alpha values given as input color.color.r = r color.color.g = g color.color.b = b color.color.a = a # Update the global color dictionary self.colors[name] = color # Actually send the colors to MoveIt! def sendColors(self): # Initialize a planning scene object p = PlanningScene() # Need to publish a planning scene diff p.is_diff = True # Append the colors from the global color dictionary for color in self.colors.values(): p.object_colors.append(color) # Publish the scene diff self.scene_pub.publish(p)
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_pick_and_place_demo') # 初始化场景对象 scene = PlanningSceneInterface() # 创建一个发布场景变化信息的发布者 self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) # 创建一个发布抓取姿态的发布者 self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10) # 创建一个存储物体颜色的字典对象 self.colors = dict() # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander(GROUP_NAME_ARM) # 初始化需要使用move group控制的机械臂中的gripper group gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.05) arm.set_goal_orientation_tolerance(0.1) # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 arm.set_pose_reference_frame(REFERENCE_FRAME) # 设置每次运动规划的时间限制:5s arm.set_planning_time(5) # 设置pick和place阶段的最大尝试次数 max_pick_attempts = 5 max_place_attempts = 5 rospy.sleep(2) # 设置场景物体的名称 table_id = 'table' box1_id = 'box1' box2_id = 'box2' target_id = 'target' # 移除场景中之前运行残留的物体 scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_world_object(target_id) # 移除场景中之前与机器臂绑定的物体 scene.remove_attached_object(GRIPPER_FRAME, target_id) rospy.sleep(1) # 控制机械臂先回到初始化位置 arm.set_named_target('home') arm.go() # 控制夹爪张开 gripper.set_joint_value_target(GRIPPER_OPEN) gripper.go() rospy.sleep(1) # 设置桌面的高度 table_ground = 0.2 # 设置table、box1和box2的三维尺寸[长, 宽, 高] table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # 将三个物体加入场景当中 table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose.position.x = 0.35 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose = PoseStamped() box1_pose.header.frame_id = REFERENCE_FRAME box1_pose.pose.position.x = 0.31 box1_pose.pose.position.y = -0.1 box1_pose.pose.position.z = table_ground + table_size[ 2] + box1_size[2] / 2.0 box1_pose.pose.orientation.w = 1.0 scene.add_box(box1_id, box1_pose, box1_size) box2_pose = PoseStamped() box2_pose.header.frame_id = REFERENCE_FRAME box2_pose.pose.position.x = 0.29 box2_pose.pose.position.y = -0.4 box2_pose.pose.position.z = table_ground + table_size[ 2] + box2_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 scene.add_box(box2_id, box2_pose, box2_size) # 将桌子设置成红色,两个box设置成橙色 self.setColor(table_id, 0.8, 0, 0, 1.0) self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # 设置目标物体的尺寸 target_size = [0.04, 0.04, 0.05] # 设置目标物体的位置,位于桌面之上两个盒子之间 target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.32 target_pose.pose.position.y = 0.0 target_pose.pose.position.z = table_ground + table_size[ 2] + target_size[2] / 2.0 target_pose.pose.orientation.w = 1.0 # 将抓取的目标物体加入场景中 scene.add_box(target_id, target_pose, target_size) # 将目标物体设置为黄色 self.setColor(target_id, 0.9, 0.9, 0, 1.0) # 将场景中的颜色设置发布 self.sendColors() # 设置支持的外观 arm.set_support_surface_name(table_id) # 设置一个place阶段需要放置物体的目标位置 place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = 0.32 place_pose.pose.position.y = -0.2 place_pose.pose.position.z = table_ground + table_size[ 2] + target_size[2] / 2.0 place_pose.pose.orientation.w = 1.0 # 将目标位置设置为机器人的抓取目标位置 grasp_pose = target_pose # 生成抓取姿态 grasps = self.make_grasps(grasp_pose, [target_id]) # 将抓取姿态发布,可以在rviz中显示 for grasp in grasps: self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) # 追踪抓取成功与否,以及抓取的尝试次数 result = None n_attempts = 0 # 重复尝试抓取,直道成功或者超多最大尝试次数 while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) result = arm.pick(target_id, grasps) rospy.sleep(0.2) # 如果pick成功,则进入place阶段 if result == MoveItErrorCodes.SUCCESS: result = None n_attempts = 0 # 生成放置姿态 places = self.make_places(place_pose) # 重复尝试放置,直道成功或者超多最大尝试次数 while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts: n_attempts += 1 rospy.loginfo("Place attempt: " + str(n_attempts)) for place in places: result = arm.place(target_id, place) if result == MoveItErrorCodes.SUCCESS: break rospy.sleep(0.2) if result != MoveItErrorCodes.SUCCESS: rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.") else: rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.") # 控制机械臂回到初始化位置 arm.set_named_target('home') arm.go() # 控制夹爪回到张开的状态 gripper.set_joint_value_target(GRIPPER_OPEN) gripper.go() rospy.sleep(1) # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Use the planning scene object to add or remove objects scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped) # Create a dictionary to hold object colors self.colors = dict() # Initialize the move group for the right arm right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.05) right_arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the right arm reference frame right_arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 10 seconds per planning attempt right_arm.set_planning_time(10) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 10 # Set a limit on the number of place attempts max_place_attempts = 5 # Give the scene a chance to catch up rospy.sleep(2) # Give each of the scene objects a unique name table_id = 'table' box1_id = 'box1' box2_id = 'box2' target_id = 'target' tool_id = 'tool' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_world_object(target_id) scene.remove_world_object(tool_id) # Remove any attached objects from a previous session scene.remove_attached_object(GRIPPER_FRAME, target_id) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "resting" pose stored in the SRDF file right_arm.set_named_target('right_start') right_arm.go() # Open the gripper to the neutral position right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Set the height of the table off the ground table_ground = 0.65 # Set the dimensions of the scene objects [l, w, h] table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # Set the target size [l, w, h] target_size = [0.02, 0.01, 0.12] # Add a table top and two boxes to the scene table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose.position.x = 0.55 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose = PoseStamped() box1_pose.header.frame_id = REFERENCE_FRAME box1_pose.pose.position.x = 0.55 box1_pose.pose.position.y = -0.1 box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0 box1_pose.pose.orientation.w = 1.0 scene.add_box(box1_id, box1_pose, box1_size) box2_pose = PoseStamped() box2_pose.header.frame_id = REFERENCE_FRAME box2_pose.pose.position.x = 0.54 box2_pose.pose.position.y = 0.13 box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 scene.add_box(box2_id, box2_pose, box2_size) # Set the target pose in between the boxes and on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.60 target_pose.pose.position.y = 0.0 target_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0 target_pose.pose.orientation.w = 1.0 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) # Make the table red and the boxes orange self.setColor(table_id, 0.8, 0, 0, 1.0) self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # Make the target yellow self.setColor(target_id, 0.9, 0.9, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the support surface name to the table object right_arm.set_support_surface_name(table_id) # Specify a pose to place the target after being picked up place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = 0.50 place_pose.pose.position.y = -0.25 place_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0 place_pose.pose.orientation.w = 1.0 # Initialize the grasp pose to the target pose grasp_pose = target_pose # Shift the grasp pose by half the width of the target to center it grasp_pose.pose.position.y -= target_size[1] / 2.0 # Generate a list of grasps grasps = self.make_grasps(grasp_pose, [target_id]) # Publish the grasp poses so they can be viewed in RViz for grasp in grasps: self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) # Track success/failure and number of attempts for pick operation result = None n_attempts = 0 # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) result = right_arm.pick(target_id, grasps) rospy.sleep(0.2) # If the pick was successful, attempt the place operation if result == MoveItErrorCodes.SUCCESS: result = None n_attempts = 0 #_------------------------now we move to the other table__________------------------------------------------- #_------------------------now we move to the other table__________------------------------------------------- # Generate valid place poses places = self.make_places(place_pose) # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts: n_attempts += 1 rospy.loginfo("Place attempt: " + str(n_attempts)) for place in places: result = right_arm.place(target_id, place) if result == MoveItErrorCodes.SUCCESS: break rospy.sleep(0.2) if result != MoveItErrorCodes.SUCCESS: rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.") else: rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.") # Return the arm to the "resting" pose stored in the SRDF file right_arm.set_named_target('right_start') right_arm.go() # Open the gripper to the neutral position right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)