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)
class MoveitPlanner: def __init__(self, manipulator_side = 'left'): # Enable the robot at start and disable at the end BaxterRobot().EnableBaxterRobot() rospy.on_shutdown(BaxterRobot().DisableBaxterRobot) # Test Open Both Grippers and Close Them # baxter_manipulator = BaxterManipulatorAndGripper() # baxter_manipulator.SetupBaxterManipualtor() # baxter_manipulator.MoveBaxterManipulatorToNeutralPose() # baxter_manipulator.SetupBaxterGrippers() # baxter_manipulator.OpenGripper('left') # baxter_manipulator.OpenGripper('right') # rospy.sleep(3.0) # baxter_manipulator.CloseGripper('left') # baxter_manipulator.CloseGripper('right') # Referenceing baxter arm self.__manipulator_name = manipulator_side self.limb = MoveGroupCommander("{}_arm".format(manipulator_side)) self.limb.set_end_effector_link("{}_gripper".format(manipulator_side)) # Baxter Model configuration with MoveIt not configured to control its gripper, # thus using Baxter SDK API gripper class baxter_manipulator = BaxterManipulatorAndGripper() baxter_manipulator.SetupBaxterChosenGripperAndManipulator(manipulator_side) self.limb.set_planner_id("RRTConnectKConfigDefualt") self.limb.set_goal_position_tolerance(0.01) self.limb.set_goal_orientation_tolerance(0.01)
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('kinect_ur3_move', anonymous=True) self.listener = tf.TransformListener() self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.initialPose = PoseStamped() self.Pose_goal = PoseStamped() self.start_goal = PoseStamped() self.current_goal = PoseStamped() self.next_pose_goal = PoseStamped() self.waypoints = [] self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) self.robot_arm.allow_replanning(True) self.robot_arm.set_goal_position_tolerance(0.01) self.robot_arm.set_goal_orientation_tolerance(0.1) def printPose(self): self.start_goal = self.robot_arm.get_current_pose("ee_link") print("Start pose:%s",self.start_goal)
def init(): # Wake up Baxter baxter_interface.RobotEnable().enable() rospy.sleep(0.25) print "Baxter is enabled" #Display initialization screen display_publsisher("/home/ubuntuz/ws_ros/src/bar_bot2/images/Init.png", 0.0) print "Intitializing clients for services" global ik_service_left ik_service_left = rospy.ServiceProxy( "ExternalTools/left/PositionKinematicsNode/IKService", SolvePositionIK) global ik_service_right ik_service_right = rospy.ServiceProxy( "ExternalTools/right/PositionKinematicsNode/IKService", SolvePositionIK) # Taken from the MoveIt Tutorials moveit_commander.roscpp_initialize(sys.argv) robot = moveit_commander.RobotCommander() # Activate Left Arm to be used with MoveIt global left_group left_group = MoveGroupCommander("left_arm") left_group.set_goal_position_tolerance(0.01) left_group.set_goal_orientation_tolerance(0.01) # Activate and calibrate left gripper global left_gripper left_gripper = baxter_interface.Gripper('left') left_gripper.calibrate() print left_gripper.parameters() #Subscribe to get end effector state global endpoint_subs endpoint_subs = rospy.Subscriber("/robot/limb/left/endpoint_state", EndpointState, end_point_callback) rospy.sleep(1) #Subscribe to external camera object tracker node global object_tracker_subs object_tracker_subs = rospy.Subscriber("/object_tracker", Pose, tracker_callback) rospy.sleep(1) #Subscribe to offset tracker node global offset_tracker_subs offset_tracker_subs = rospy.Subscriber("/arm_offset_tracker", Point, offset_callback) rospy.sleep(1) #Subscribe to right arm navigator global navigator_sub navigator_sub = rospy.Subscriber("/robot/navigators/right_navigator/state", NavigatorState, navigator_callback) rospy.sleep(1)
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(): #Wake up Baxter baxter_interface.RobotEnable().enable() rospy.sleep(0.25) print "Baxter is enabled" print "Intitializing clients for services" global ik_service_left ik_service_left = rospy.ServiceProxy( "ExternalTools/left/PositionKinematicsNode/IKService", SolvePositionIK) global ik_service_right ik_service_right = rospy.ServiceProxy( "ExternalTools/right/PositionKinematicsNode/IKService", SolvePositionIK) global obj_loc_service obj_loc_service = rospy.ServiceProxy("object_location_service", ObjLocation) global stopflag stopflag = False #Taken from the MoveIt Tutorials moveit_commander.roscpp_initialize(sys.argv) robot = moveit_commander.RobotCommander() global scene scene = moveit_commander.PlanningSceneInterface() #Activate Left Arm to be used with MoveIt global left_group left_group = MoveGroupCommander("left_arm") left_group.set_goal_position_tolerance(0.01) left_group.set_goal_orientation_tolerance(0.01) global right_group right_group = MoveGroupCommander("right_arm") pose_right = Pose() pose_right.position = Point(0.793, -0.586, 0.329) pose_right.orientation = Quaternion(1.0, 0.0, 0.0, 0.0) request_pose(pose_right, "right", right_group) global left_gripper left_gripper = baxter_interface.Gripper('left') left_gripper.calibrate() print left_gripper.parameters() global end_effector_subs end_effector_subs = rospy.Subscriber( "/robot/end_effector/left_gripper/state", EndEffectorState, end_effector_callback) rospy.sleep(1) global pubpic pubpic = rospy.Publisher('/robot/xdisplay', Image, latch=True, queue_size=1) rospy.sleep(1)
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)
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 MoveItArm: """ Represents Baxter's Arm in MoveIt! world. Represents a hand of Baxter through MoveIt! Group. Some reusable code has been packed together into this class to make the code more readable. """ def __init__(self, side_name): """Will configure and initialise Baxter's hand.""" self._side_name = side_name # This is the reference to Baxter's arm. self.limb = MoveGroupCommander("{}_arm".format(side_name)) self.limb.set_end_effector_link("{}_gripper".format(side_name)) # Unfortunetly, MoveIt was not able to make the gripper work, neither # did was able to find a very good pose using Forward Kinematics, # so instead the Baxter SDK is used here to do these two jobs. self.gripper = Gripper(side_name, CHECK_VERSION) self.gripper.calibrate() self._limb = Limb(side_name) # This solver seems to be better for finding solution among obstacles self.limb.set_planner_id("RRTConnectkConfigDefault") # Error tollerance should be as low as possible for better accuracy. self.limb.set_goal_position_tolerance(0.01) self.limb.set_goal_orientation_tolerance(0.01) def __str__(self): """ String representation of the arm. String representation of the arm, either 'left' or 'right' string will be returned. Useful when the string representation of the arm is needed, like when accessing the IK solver. """ return self._side_name def is_left(self): """Will return True if this is the left arm, false otherwise.""" return True if self._side_name == "left" else False def is_right(self): """Will return True if this is the right arm, false otherwise.""" return True if self._side_name == "right" else False def open_gripper(self): """Will open Baxter's gripper on his active hand.""" # Block ensures that the function does not return until the operation # is completed. self.gripper.open(block=True) def close_gripper(self): """Will close Baxter's gripper on his active hand.""" # Block ensures that the function does not return until the operation # is completed. self.gripper.close(block=True)
def init(): #Wake up Baxter baxter_interface.RobotEnable().enable() rospy.sleep(0.25) print "Baxter is enabled" print "Intitializing clients for services" global ik_service_left ik_service_left = rospy.ServiceProxy( "ExternalTools/left/PositionKinematicsNode/IKService", SolvePositionIK) global ik_service_right ik_service_right = rospy.ServiceProxy( "ExternalTools/right/PositionKinematicsNode/IKService", SolvePositionIK) global obj_loc_service obj_loc_service = rospy.ServiceProxy( "object_location_service", ObjLocation) global stopflag stopflag = False #Taken from the MoveIt Tutorials moveit_commander.roscpp_initialize(sys.argv) robot = moveit_commander.RobotCommander() global scene scene = moveit_commander.PlanningSceneInterface() #Activate Left Arm to be used with MoveIt global left_group left_group = MoveGroupCommander("left_arm") left_group.set_goal_position_tolerance(0.01) left_group.set_goal_orientation_tolerance(0.01) global right_group right_group = MoveGroupCommander("right_arm") pose_right = Pose() pose_right.position = Point(0.793, -0.586, 0.329) pose_right.orientation = Quaternion(1.0, 0.0, 0.0, 0.0) request_pose(pose_right, "right", right_group) global left_gripper left_gripper = baxter_interface.Gripper('left') left_gripper.calibrate() print left_gripper.parameters() global end_effector_subs end_effector_subs = rospy.Subscriber("/robot/end_effector/left_gripper/state", EndEffectorState, end_effector_callback) rospy.sleep(1) global pubpic pubpic = rospy.Publisher('/robot/xdisplay', Image, latch=True, queue_size=1) rospy.sleep(1)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_speed_demo') # Initialize the move group for the right arm arm = MoveGroupCommander('manipulator') # Get the name of the end-effector link end_effector_link = arm.get_end_effector_link() # Allow replanning to increase the odds of a solution arm.allow_replanning(True) # Allow some leeway in position(meters) and orientation (radians) arm.set_goal_position_tolerance(0.001) arm.set_goal_orientation_tolerance(0.01) # Start the arm in the "home" pose stored in the SRDF file arm.set_named_target('home') arm.go() # 设置机械臂的目标位置,使用六轴的位置数据进行描述(单位:弧度) joint_positions = [0.391410, -0.676384, -0.376217, 0.0, 1.052834, 0.454125] arm.set_joint_value_target(joint_positions) # 控制机械臂完成运动 arm.go() # Start the arm in the "home" pose stored in the SRDF file arm.set_named_target('home') arm.go() # Get back the planned trajectory arm.set_joint_value_target(joint_positions) traj = arm.plan() # Scale the trajectory speed by a factor of 0.25 new_traj = scale_trajectory_speed(traj, 0.25) # Execute the new trajectory arm.execute(new_traj) rospy.sleep(1) arm.set_named_target('home') arm.go() # Exit 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.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): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Initialize the move group for the right arm right_arm = MoveGroupCommander('right_arm') # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Allow some leeway in position(meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.02) right_arm.set_goal_orientation_tolerance(0.1) # Start the arm in the "resting" pose stored in the SRDF file right_arm.set_named_target('rest') right_arm.go() # Move to the named "straight_forward" pose stored in the SRDF file right_arm.set_named_target('wave') right_arm.go() rospy.sleep(2) # Move back to the resting position at roughly 1/4 speed right_arm.set_named_target('rest') # Get back the planned trajectory traj = right_arm.plan() # Set the trajectory speed new_traj = create_tracking_trajectory(traj, 1.0) rospy.loginfo(new_traj) # Execute the new trajectory right_arm.execute(new_traj) rospy.sleep(1) # Exit 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_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
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') 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(0.5) # 获取终端link的名称 eef_link = arm.get_end_effector_link() scene = moveit_commander.PlanningSceneInterface() print "[INFO] Current pose:", arm.get_current_pose(eef_link).pose # 控制机械臂运动到之前设置的姿态 # arm.set_named_target('pick_6') # arm.set_named_target('home') # arm.go() self.box_name = '' self.scene = scene self.group = arm self.robot = robot 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 talker_by13(): #init moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_fk_demo') #cartesian = rospy.get_param('~cartesian', True) arm = MoveGroupCommander('manipulator') arm.set_pose_reference_frame('base_link') arm.allow_replanning(True) arm.set_goal_position_tolerance(0.001) arm.set_goal_orientation_tolerance(0.001) # arm.set_max_acceleration_scaling_factor(0.5) #arm.set_max_velocity_scaling_factor(0.5) end_effector_link = arm.get_end_effector_link() #arm.set_named_target('home') arm.set_named_target('up') arm.go() rospy.sleep(2) target_pose = PoseStamped() target_pose.header.frame_id = 'base_link' target_pose.header.stamp = rospy.Time.now() target_pose.pose.position.x = 0.86 target_pose.pose.position.y = 0.25 target_pose.pose.position.z = 0.02832 target_pose.pose.orientation.x = 0 target_pose.pose.orientation.y = 0 target_pose.pose.orientation.z = 0 target_pose.pose.orientation.w = 1 #next: find workspace arm.set_start_state_to_current_state() arm.set_pose_target(target_pose, end_effector_link) traj = arm.plan() arm.execute(traj) #arm.shift_pose_target(2,-0.05,end_effector_link) #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_demo') # Initialize the move group for the right arm right_arm = MoveGroupCommander('right_arm') # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Allow some leeway in position(meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.02) right_arm.set_goal_orientation_tolerance(0.1) # Start the arm in the "resting" pose stored in the SRDF file right_arm.set_named_target('resting') right_arm.go() # Move to the named "straight_forward" pose stored in the SRDF file right_arm.set_named_target('straight_forward') right_arm.go() rospy.sleep(2) # Move back to the resting position at roughly 1/4 speed right_arm.set_named_target('resting') # Get back the planned trajectory traj = right_arm.plan() # Scale the trajectory speed by a factor of 0.25 new_traj = scale_trajectory_speed(traj, 0.25) # Execute the new trajectory right_arm.execute(new_traj) rospy.sleep(1) # Exit MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
class Robot: def __init__(self, config, debug=0): print "[INFO] Initialise Robot" self.DEBUG = debug # configuration self.config = config # initialise move groups self.arm = MoveGroupCommander(ARM_GROUP) self.gripper = MoveGroupCommander(GRIPPER_GROUP) # initialise pick and place manager if self.DEBUG is 1: # verbose mode self.pick_and_place = PickPlaceInterface(ARM_GROUP, GRIPPER_GROUP, False, True) else: # non verbose mode self.pick_and_place = PickPlaceInterface(ARM_GROUP, GRIPPER_GROUP, False, False) # tolerance: position (in m), orientation (in rad) self.arm.set_goal_position_tolerance(0.01) self.arm.set_goal_orientation_tolerance(0.1) self.place_manager = None self.pick_manager = None self.initialise_move_actions() self.start_position() """Initialise the place and pick manager. """ def initialise_move_actions(self): self.place_manager = PlaceManager(self.arm, self.pick_and_place, self.config, self.DEBUG) self.pick_manager = PickManager(self.arm, self.pick_and_place, self.DEBUG) """Move robot arm to "start position". """ def start_position(self): self.arm.set_named_target(START_POSITION) self.arm.go() rospy.sleep(2)
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_pick_and_place_demo') # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander(GROUP_NAME_ARM) self.arm = arm # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.01) # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 arm.set_pose_reference_frame(REFERENCE_FRAME)
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 Robot: def __init__(self, config, debug=0): print "[INFO] Initialise Robot" self.DEBUG = debug # configuration self.config = config # initialise move groups self.arm = MoveGroupCommander(ARM_GROUP) self.gripper = MoveGroupCommander(GRIPPER_GROUP) # initialise pick and place manager if self.DEBUG is 1: # verbose mode self.pick_and_place = PickPlaceInterface(ARM_GROUP, GRIPPER_GROUP, False, True) else: # non verbose mode self.pick_and_place = PickPlaceInterface(ARM_GROUP, GRIPPER_GROUP, False, False) # tolerance: position (in m), orientation (in rad) self.arm.set_goal_position_tolerance(0.01) self.arm.set_goal_orientation_tolerance(0.1) self.place_manager = None self.pick_manager = None self.initialise_move_actions() self.start_position() """Initialise the place and pick manager. """ def initialise_move_actions(self): self.place_manager = PlaceManager(self.arm, self.pick_and_place, self.config, self.DEBUG) self.pick_manager = PickManager(self.arm, self.pick_and_place, self.DEBUG) """Move robot arm to "start position". """ def start_position(self): self.arm.set_named_target(START_POSITION) self.arm.go() rospy.sleep(2)
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) # 是否需要使用笛卡尔空间的运动规划 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.4) 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() self.group = arm self.eef_link = eef_link self.reference_frame = reference_frame self.moveit_commander = moveit_commander self.listener = tf.TransformListener() self.broadcaster = tf.TransformBroadcaster()
def InitializeMoveitCommander(): #First initialize moveit_commander print "============ Starting tutorial setup" moveit_commander.roscpp_initialize(sys.argv) #Instantiate a RobotCommander object. This object is an interface to the robot as a whole. robot = moveit_commander.RobotCommander() #Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up. print "============ Waiting for RVIZ..." rospy.sleep(1) print "============ Starting tutorial " #Instantiate a PlanningSceneInterface object. This object is an interface to the world surrounding the robot. scene = moveit_commander.PlanningSceneInterface() #Instantiate a MoveGroupCommander object. This object is an interface to one group of joints. In this case the group is the joints in the left arm. This interface can be used to plan and execute motions on the left arm. global group_both_arms, group_left_arm, group_right_arm group_both_arms = MoveGroupCommander("both_arms") group_both_arms.set_goal_position_tolerance(0.01) group_both_arms.set_goal_orientation_tolerance(0.01) group_left_arm = MoveGroupCommander("left_arm") group_left_arm.set_goal_position_tolerance(0.01) group_left_arm.set_goal_orientation_tolerance(0.01) group_right_arm = MoveGroupCommander("right_arm") group_right_arm.set_goal_position_tolerance(0.01) group_right_arm.set_goal_orientation_tolerance(0.01) #We create this DisplayTrajectory publisher which is used below to publish trajectories for RVIZ to visualize. display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory) # Obtain current poses of left and right end-effectors: [x,y,z,roll,pitch,yaw].T P_left_pose = group_left_arm.get_current_pose() P_right_pose = group_right_arm.get_current_pose() P_left_euler = group_left_arm.get_current_rpy() P_right_euler = group_right_arm.get_current_rpy() global P_left_current, P_right_current P_left_current = np.array([[P_left_pose.pose.position.x], [P_left_pose.pose.position.y], [P_left_pose.pose.position.z], [P_left_euler[0]], [P_left_euler[1]], [P_left_euler[2]]]) P_right_current = np.array([[P_right_pose.pose.position.x], [P_right_pose.pose.position.y], [P_right_pose.pose.position.z], [P_right_euler[0]], [P_right_euler[1]], [P_right_euler[1]]]) print "\nCurrent pose of left and right EE: \n", np.transpose( P_left_current), "\n", np.transpose(P_right_current)
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) cartesian = rospy.get_param('~cartesian', True) # Connect to the arm move group arm = MoveGroupCommander('arm') # Allow replanning to increase the odds of a solution arm.allow_replanning(True) # Set the right arm reference frame arm.set_pose_reference_frame('base_link') # Allow some leeway in position(meters) and orientation (radians) arm.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.05) # Get the name of the end-effector link end_effector_link = arm.get_end_effector_link() # Set an initial position for the arm start_position = [0.0, 0.5, -0.0074579719079, -1.67822729461, -3.1415174069, -1.1, 3.1415174069] # Set the goal pose of the end effector to the stored pose arm.set_joint_value_target(start_position) # Plan and execute a trajectory to the goal configuration arm.go() # Get the current pose so we can add it as a waypoint start_pose = arm.get_current_pose(end_effector_link).pose # Initialize the waypoints list waypoints = [] # Set the first waypoint to be the starting pose if cartesian: # Append the pose to the waypoints list waypoints.append(start_pose) wpose = deepcopy(start_pose) # Move end effector to the right 0.3 meters wpose.position.y -= 0.3 if cartesian: # Append the pose to the waypoints list waypoints.append(deepcopy(wpose)) else: arm.set_pose_target(wpose) arm.go() rospy.sleep(1) # Move end effector up and back wpose.position.x -= 0.2 wpose.position.z += 0.3 if cartesian: # Append the pose to the waypoints list waypoints.append(deepcopy(wpose)) else: arm.set_pose_target(wpose) arm.go() rospy.sleep(1) if cartesian: # Append the pose to the waypoints list 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 # Set the internal state to the current state arm.set_start_state_to_current_state() # Plan the Cartesian path connecting the waypoints while fraction < 1.0 and attempts < maxtries: (plan, fraction) = arm.compute_cartesian_path ( waypoints, # waypoint poses 0.025, # eef_step 0.0, # jump_threshold True) # avoid_collisions # Increment the number of attempts attempts += 1 # Print out a progress message if attempts % 10 == 0: rospy.loginfo("Still trying after " + str(attempts) + " attempts...") # If we have a complete plan, execute the trajectory 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.") # 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)
class ArmTracker: def __init__(self): rospy.init_node('arm_tracker') rospy.on_shutdown(self.shutdown) # Maximum distance of the target before the arm will lower self.max_target_dist = 1.2 # Arm length to center of gripper frame self.arm_length = 0.4 # Distance between the last target and the new target before we move the arm self.last_target_threshold = 0.01 # Distance between target and end-effector before we move the arm self.target_ee_threshold = 0.025 # Initialize the move group for the right arm self.right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Set the reference frame for pose targets self.reference_frame = REFERENCE_FRAME # Keep track of the last target pose self.last_target_pose = PoseStamped() # Set the right arm reference frame accordingly self.right_arm.set_pose_reference_frame(self.reference_frame) # Allow replanning to increase the chances of a solution self.right_arm.allow_replanning(False) # Set a position tolerance in meters self.right_arm.set_goal_position_tolerance(0.05) # Set an orientation tolerance in radians self.right_arm.set_goal_orientation_tolerance(0.2) # What is the end effector link? self.ee_link = self.right_arm.get_end_effector_link() # Create the transform listener self.listener = tf.TransformListener() # Queue up some tf data... rospy.sleep(3) # Set the gripper target to closed position using a joint value target right_gripper.set_joint_value_target(GRIPPER_CLOSED) # Plan and execute the gripper motion right_gripper.go() rospy.sleep(1) # Subscribe to the target topic rospy.wait_for_message('/target_pose', PoseStamped) # Use queue_size=1 so we don't pile up outdated target messages self.target_subscriber = rospy.Subscriber('/target_pose', PoseStamped, self.update_target_pose, queue_size=1) rospy.loginfo("Ready for action!") while not rospy.is_shutdown(): try: target = self.target except: rospy.sleep(0.5) continue # Timestamp the target with the current time target.header.stamp = rospy.Time() # Get the target pose in the right_arm shoulder lift frame #target_arm = self.listener.transformPose('right_arm_shoulder_pan_link', target) target_arm = self.listener.transformPose('right_arm_base_link', target) # Convert the position values to a Python list p0 = [target_arm.pose.position.x, target_arm.pose.position.y, target_arm.pose.position.z] # Compute the distance between the target and the shoulder link dist_target_shoulder = euclidean(p0, [0, 0, 0]) # If the target is too far away, then lower the arm if dist_target_shoulder > self.max_target_dist: rospy.loginfo("Target is too far away") self.right_arm.set_named_target('resting') self.right_arm.go() rospy.sleep(1) continue # Transform the pose to the base reference frame target_base = self.listener.transformPose(self.reference_frame, target) # Compute the distance between the current target and the last target p1 = [target_base.pose.position.x, target_base.pose.position.y, target_base.pose.position.z] p2 = [self.last_target_pose.pose.position.x, self.last_target_pose.pose.position.y, self.last_target_pose.pose.position.z] dist_last_target = euclidean(p1, p2) # Move the arm only if we are far enough away from the previous target if dist_last_target < self.last_target_threshold: rospy.loginfo("Still close to last target") rospy.sleep(0.5) continue # Get the pose of the end effector in the base reference frame ee_pose = self.right_arm.get_current_pose(self.ee_link) # Convert the position values to a Python list p3 = [ee_pose.pose.position.x, ee_pose.pose.position.y, ee_pose.pose.position.z] # Compute the distance between the target and the end-effector dist_target = euclidean(p1, p3) # Only move the arm if we are far enough away from the target if dist_target < self.target_ee_threshold: rospy.loginfo("Already close enough to target") rospy.sleep(1) continue # We want the gripper somewhere on the line connecting the shoulder and the target. # Using a parametric form of the line, the parameter ranges from 0 to the # minimum of the arm length and the distance to the target. t_max = min(self.arm_length, dist_target_shoulder) # Bring it back 10% so we don't collide with the target t = 0.9 * t_max # Now compute the target positions from the parameter try: target_arm.pose.position.x *= (t / dist_target_shoulder) target_arm.pose.position.y *= (t / dist_target_shoulder) target_arm.pose.position.z *= (t / dist_target_shoulder) except: rospy.sleep(1) rospy.loginfo("Exception!") continue # Transform to the base_footprint frame target_ee = self.listener.transformPose(self.reference_frame, target_arm) # Set the target gripper orientation to be horizontal target_ee.pose.orientation.x = 0 target_ee.pose.orientation.y = 0 target_ee.pose.orientation.z = 0 target_ee.pose.orientation.w = 1 # Update the current start state self.right_arm.set_start_state_to_current_state() # Set the target pose for the end-effector self.right_arm.set_pose_target(target_ee, self.ee_link) # Plan and execute the trajectory success = self.right_arm.go() if success: # Store the current target as the last target self.last_target_pose = target # Pause a bit between motions to keep from locking up rospy.sleep(0.5) def update_target_pose(self, target): self.target = target def relax_all_servos(self): command = 'rosrun donaxi_dynamixels arbotix_relax_all_servos.py' args = shlex.split(command) subprocess.Popen(args) def shutdown(self): # Stop any further target messages from being processed self.target_subscriber.unregister() # Stop any current arm movement self.right_arm.stop() # Move to the resting position self.right_arm.set_named_target('resting') self.right_arm.go() # Relax the servos self.relax_all_servos() 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) # 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): # 初始化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.2) arm.set_goal_orientation_tolerance(0.1) # 设置允许的最大速度和加速度 arm.set_max_acceleration_scaling_factor(0.1) arm.set_max_velocity_scaling_factor(0.1) # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() print(end_effector_link) # 控制机械臂先回到初始化位置 # 获取当前位姿数据最为机械臂运动的起始位姿 start_pose = arm.get_current_pose(end_effector_link).pose print(start_pose) # 初始化路点列表 waypoints = [] os.system("rosrun pick_test send_gripper.py --value 0.0") # 将初始位姿加入路点列表 # arm.set_named_target('test5') # arm.go() # rospy.sleep(1) ###### marker = rospy.Subscriber("/ar_pose_marker", AlvarMarkers, getCarrot) listener = tf.TransformListener() r = rospy.Rate(10) while not rospy.is_shutdown(): try: (trans, rot) = listener.lookupTransform('/base_link', '/ar_marker_4', rospy.Time(0)) break except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue ###### # 设置路点数据,并加入路点列表 wpose = deepcopy(start_pose) print carrot wpose.position.z -= carrot.z waypoints.append(deepcopy(wpose)) # wpose.position.z -= carrot.z+0.1 # wpose.position.y += carrot.y # waypoints.append(deepcopy(wpose)) 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.") rospy.sleep(1) if fraction < 1.0: moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0) # 控制机械臂先回到初始化位置 waypoints = [] test_pose = arm.get_current_pose(end_effector_link).pose wpose = deepcopy(test_pose) wpose.position.x -= carrot.x + 0.03 waypoints.append(deepcopy(wpose)) 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.02, # 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.") rospy.sleep(1) if fraction < 1.0: moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0) ######## os.system("rosrun pick_test send_gripper.py --value 0.8") ######## waypoints = [] pick_pose = arm.get_current_pose(end_effector_link).pose wpose = deepcopy(pick_pose) wpose.position.z += 0.1 waypoints.append(deepcopy(wpose)) 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.02, # 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.") rospy.sleep(1) if fraction < 1.0: moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0) # 关闭并退出moveit moveit_commander.roscpp_shutdown() 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): # 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): # 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') # 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)
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 __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('TCP_Move', anonymous=True) # 是否需要使用笛卡尔空间的运动规划 cartesian = rospy.get_param('~cartesian', True) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('xarm6') # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 arm.set_pose_reference_frame('link_base') # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.001) arm.set_goal_orientation_tolerance(0.001) # 设置允许的最大速度和加速度 arm.set_max_acceleration_scaling_factor(0.1) arm.set_max_velocity_scaling_factor(0.1) # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 控制机械臂先回到初始化位置 arm.set_named_target('home') arm.go() rospy.sleep(1) # 角度弧度转换 j1 = 90.0 / 180 * math.pi j2 = -18.6 / 180 * math.pi j3 = -28.1 / 180 * math.pi j4 = 1.0 / 180 * math.pi j5 = 47.6 / 180 * math.pi j6 = -0.9 / 180 * math.pi # 设置机械臂的目标位置,使用六轴的位置数据进行描述(单位:弧度) joint_positions = [j1, j2, j3, j4, j5, j6] arm.set_joint_value_target(joint_positions) arm.go() rospy.sleep(1) # 向下按压门把手 current_pose = arm.get_current_joint_values() current_pose[4] += (20.0 / 180.0) * math.pi arm.set_joint_value_target(current_pose) arm.go() rospy.sleep(1) #推开门 current_pose = arm.get_current_joint_values() current_pose[0] -= (42.0 / 180.0) * math.pi current_pose[1] += (2.0 / 180.0) * math.pi current_pose[2] -= (11.4 / 180.0) * math.pi arm.set_joint_value_target(current_pose) arm.go() rospy.sleep(1) # 控制机械臂先回到初始化位置 arm.set_named_target('home') arm.go() 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_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') # 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)
class ArmTracker: def __init__(self): rospy.init_node('arm_tracker') rospy.on_shutdown(self.shutdown) # Maximum distance of the target before the arm will lower self.max_target_dist = 1.2 # Arm length to center of gripper frame self.arm_length = 0.4 # Distance between the last target and the new target before we move the arm self.last_target_threshold = 0.01 # Distance between target and end-effector before we move the arm self.target_ee_threshold = 0.025 # Initialize the move group for the left arm self.left_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the left gripper left_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Set the reference frame for pose targets self.reference_frame = REFERENCE_FRAME # Keep track of the last target pose self.last_target_pose = PoseStamped() # Set the left arm reference frame accordingly self.left_arm.set_pose_reference_frame(self.reference_frame) # Allow replanning to increase the chances of a solution self.left_arm.allow_replanning(False) # Set a position tolerance in meters self.left_arm.set_goal_position_tolerance(0.05) # Set an orientation tolerance in radians self.left_arm.set_goal_orientation_tolerance(0.2) # What is the end effector link? self.ee_link = self.left_arm.get_end_effector_link() # Create the transform listener self.listener = tf.TransformListener() # Queue up some tf data... rospy.sleep(3) # Set the gripper target to closed position using a joint value target left_gripper.set_joint_value_target(GRIPPER_CLOSED) # Plan and execute the gripper motion left_gripper.go() rospy.sleep(1) # Subscribe to the target topic rospy.wait_for_message('/target_pose', PoseStamped) # Use queue_size=1 so we don't pile up outdated target messages self.target_subscriber = rospy.Subscriber('/target_pose', PoseStamped, self.update_target_pose, queue_size=1) rospy.loginfo("Ready for action!") while not rospy.is_shutdown(): try: target = self.target except: rospy.sleep(0.5) continue # Timestamp the target with the current time target.header.stamp = rospy.Time() # Get the target pose in the left_arm shoulder lift frame #target_arm = self.listener.transformPose('left_arm_shoulder_pan_link', target) target_arm = self.listener.transformPose('left_rotate', target) # Convert the position values to a Python list p0 = [ target_arm.pose.position.x, target_arm.pose.position.y, target_arm.pose.position.z ] # Compute the distance between the target and the shoulder link dist_target_shoulder = euclidean(p0, [0, 0, 0]) # If the target is too far away, then lower the arm if dist_target_shoulder > self.max_target_dist: rospy.loginfo("Target is too far away") self.left_arm.set_named_target('l_start') self.left_arm.go() rospy.sleep(1) continue # Transform the pose to the base reference frame target_base = self.listener.transformPose(self.reference_frame, target) # Compute the distance between the current target and the last target p1 = [ target_base.pose.position.x, target_base.pose.position.y, target_base.pose.position.z ] p2 = [ self.last_target_pose.pose.position.x, self.last_target_pose.pose.position.y, self.last_target_pose.pose.position.z ] dist_last_target = euclidean(p1, p2) # Move the arm only if we are far enough away from the previous target if dist_last_target < self.last_target_threshold: rospy.loginfo("Still close to last target") rospy.sleep(0.5) continue # Get the pose of the end effector in the base reference frame ee_pose = self.left_arm.get_current_pose(self.ee_link) # Convert the position values to a Python list p3 = [ ee_pose.pose.position.x, ee_pose.pose.position.y, ee_pose.pose.position.z ] # Compute the distance between the target and the end-effector dist_target = euclidean(p1, p3) # Only move the arm if we are far enough away from the target if dist_target < self.target_ee_threshold: rospy.loginfo("Already close enough to target") rospy.sleep(1) continue # We want the gripper somewhere on the line connecting the shoulder and the target. # Using a parametric form of the line, the parameter ranges from 0 to the # minimum of the arm length and the distance to the target. t_max = min(self.arm_length, dist_target_shoulder) # Bring it back 10% so we don't collide with the target t = 0.9 * t_max # Now compute the target positions from the parameter try: target_arm.pose.position.x *= (t / dist_target_shoulder) target_arm.pose.position.y *= (t / dist_target_shoulder) target_arm.pose.position.z *= (t / dist_target_shoulder) except: rospy.sleep(1) rospy.loginfo("Exception!") continue # Transform to the base_footprint frame target_ee = self.listener.transformPose(self.reference_frame, target_arm) # Set the target gripper orientation to be horizontal target_ee.pose.orientation.x = 0 target_ee.pose.orientation.y = 0 target_ee.pose.orientation.z = 0 target_ee.pose.orientation.w = 1 # Update the current start state self.left_arm.set_start_state_to_current_state() # Set the target pose for the end-effector self.left_arm.set_pose_target(target_ee, self.ee_link) # Plan and execute the trajectory success = self.left_arm.go() if success: # Store the current target as the last target self.last_target_pose = target # Pause a bit between motions to keep from locking up rospy.sleep(0.5) def update_target_pose(self, target): self.target = target def relax_all_servos(self): command = 'rosrun rbx2_dynamixels arbotix_relax_all_servos.py' args = shlex.split(command) subprocess.Popen(args) def shutdown(self): # Stop any further target messages from being processed self.target_subscriber.unregister() # Stop any current arm movement self.left_arm.stop() # Move to the r_start position self.left_arm.set_named_target('r_start') self.left_arm.go() # Relax the servos self.relax_all_servos() 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_demo', anonymous=True) cartesian = rospy.get_param('~cartesian', True) # Connect to the right_arm move group right_arm = MoveGroupCommander('right_arm') # 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('base_footprint') # Allow some leeway in position(meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.01) 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 "straight_forward" configuration stored in the SRDF file right_arm.set_named_target('straight_forward') # Plan and execute a trajectory to the goal configuration right_arm.go() # Get the current pose so we can add it as a waypoint start_pose = right_arm.get_current_pose(end_effector_link).pose # Initialize the waypoints list waypoints = [] # Set the first waypoint to be the starting pose if cartesian: # Append the pose to the waypoints list waypoints.append(start_pose) wpose = deepcopy(start_pose) # Set the next waypoint back 0.2 meters and right 0.2 meters wpose.position.x -= 0.2 wpose.position.y -= 0.2 if cartesian: # Append the pose to the waypoints list waypoints.append(deepcopy(wpose)) else: right_arm.set_pose_target(wpose) right_arm.go() rospy.sleep(1) # Set the next waypoint to the right 0.15 meters wpose.position.x += 0.05 wpose.position.y += 0.15 wpose.position.z -= 0.15 if cartesian: # Append the pose to the waypoints list waypoints.append(deepcopy(wpose)) else: right_arm.set_pose_target(wpose) right_arm.go() rospy.sleep(1) if cartesian: # Append the pose to the waypoints list waypoints.append(deepcopy(start_pose)) else: right_arm.set_pose_target(start_pose) right_arm.go() rospy.sleep(1) if cartesian: fraction = 0.0 maxtries = 100 attempts = 0 # Set the internal state to the current state right_arm.set_start_state_to_current_state() # Plan the Cartesian path connecting the waypoints while fraction < 1.0 and attempts < maxtries: (plan, fraction) = right_arm.compute_cartesian_path ( waypoints, # waypoint poses 0.01, # eef_step 0.0, # jump_threshold True) # avoid_collisions # Increment the number of attempts attempts += 1 # Print out a progress message if attempts % 10 == 0: rospy.loginfo("Still trying after " + str(attempts) + " attempts...") # If we have a complete plan, execute the trajectory if fraction == 1.0: rospy.loginfo("Path computed successfully. Moving the arm.") right_arm.execute(plan) rospy.loginfo("Path execution complete.") else: rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.") # Move normally back to the 'resting' position right_arm.set_named_target('resting') right_arm.go() rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit MoveIt moveit_commander.os._exit(0)