def main(): # Initiate node, listener, and gripper rospy.init_node("testing") listener = tf.TransformListener() right_gripper = robot_gripper.Gripper('right') while not rospy.is_shutdown(): right_gripper.open() try: # Create and initialize the MoveGroup group = MoveGroupCommander("right_arm") init_move_group(group, position_tolerance=0.01) print group.get_end_effector_link() # for dbugging ################# # end_location = get_artag_location(listener, "ar_marker_20") # upper_left = get_artag_location(listener, "ar_marker_23") # lower_right = get_artag_location(listener, "ar_marker_22") # cg = RobotCheckers(upper_left, lower_right) # end_location = cg.location(1, 1) # print end_location ################# # move = cg.detect_opponent_move(listener) # print move # move_group_to(group, 0.609, -0.131, -0.157) move_group_to(group, 0.406, -0.140, -0.188) # move_checkers_piece(group, right_gripper, listener, 0, end_location=end_location) except rospy.ServiceException, e: print "Service call failed: %s" % e sys.exit()
def execute(self, userdata): mc = MoveGroupCommander("r1_arm") userdata.robot_1_state = mc.get_current_joint_values() userdata.ik_link_1 = mc.get_end_effector_link() userdata.robot_1_pose = mc.get_current_pose() mc = MoveGroupCommander("r2_arm") userdata.robot_2_state = mc.get_current_joint_values() userdata.ik_link_2 = mc.get_end_effector_link() userdata.robot_2_pose = mc.get_current_pose() mc = MoveGroupCommander("arms") userdata.complete_state = mc.get_current_joint_values() return 'succeeded'
def transform_to_tip(group, ee_frame, goal): """ Transform the goal from the ee_frame to the tip link which is resolved from planning group return (new_goal, tip_frame) """ mc = MoveGroupCommander(group) tip_frame = mc.get_end_effector_link() ee_pose = mc.get_current_pose(ee_frame) tip_pose = mc.get_current_pose(tip_frame) kdl_ee = tf_conversions.fromMsg(ee_pose.pose) kdl_tip = tf_conversions.fromMsg(tip_pose.pose) if isinstance(goal, geometry_msgs.msg.Pose): kdl_goal = tf_conversions.fromMsg(goal) elif isinstance(goal, geometry_msgs.msg.PoseStamped): kdl_goal = tf_conversions.fromMsg(goal.pose) else: rospy.logerr("Unknown pose type, only Pose and PoseStamped is allowed") rospy.logerr(str(type(goal))) return (None, tip_frame) grip = kdl_tip.Inverse() * kdl_ee kdl_pose = kdl_goal * grip.Inverse() return (tf_conversions.toMsg(kdl_pose), tip_frame)
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_cartesian_demo', anonymous=True) # 是否需要使用笛卡尔空间的运动规划 cartesian = rospy.get_param('~cartesian', True) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('arm') # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 # arm.set_goal_position_tolerance(0.00001) # arm.set_goal_orientation_tolerance(0.00001) arm.set_max_velocity_scaling_factor(0.5) arm.set_max_acceleration_scaling_factor(1.0) arm.set_planning_time(0.5) # 规划时间限制为2秒 arm.allow_replanning(True) # 当运动规划失败后,是否允许重新规划 # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) # 获取终端link的名称 eef_link = arm.get_end_effector_link() self.group = arm self.eef_link = eef_link self.reference_frame = reference_frame self.moveit_commander = moveit_commander self.listener = tf.TransformListener()
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) # Initialize the ROS node rospy.init_node('get_joint_states', anonymous=True) # Initialize the MoveIt! commander for the right arm right_arm = MoveGroupCommander('tx90_arm') # Get the end-effector link end_effector_link = right_arm.get_end_effector_link() # Joints are stored in the order they appear in the kinematic chain joint_names = right_arm.get_active_joints() # Display the joint names rospy.loginfo("Joint names:\n" + str(joint_names)) # Get the current joint angles joint_values = right_arm.get_current_joint_values() # Display the joint values rospy.loginfo("Joint values:\n" + str(joint_values) + "\n") # # Get the end-effector pose ee_pose = right_arm.get_current_pose(end_effector_link) # Display the end-effector pose rospy.loginfo("End effector pose:\n" + str(ee_pose)) moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self, pose): self.check_collision = False # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_obstacles_demo') # 初始化场景对象 scene = PlanningSceneInterface() # 创建一个发布场景变化信息的发布者 self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) # 创建一个存储物体颜色的字典对象 self.colors = dict() # 等待场景准备就绪 rospy.sleep(1) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('arm') # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.05) # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) # 设置每次运动规划的时间限制:5s arm.set_planning_time(5) # 将场景中的颜色设置发布 self.sendColors() rospy.sleep(5) joint_positions = pose arm.set_joint_value_target(joint_positions) # 控制机械臂完成运动 if arm.go(): pass else: self.check_collision = True rospy.sleep(1) # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('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): # 初始化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() # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) # 获取终端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
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, ())
def get_ur_pose(req): manipulator = MoveGroupCommander('manipulator') end_effector_link = manipulator.get_end_effector_link() ee_pose = manipulator.get_current_pose(end_effector_link) rospy.loginfo("End effector pose:\n" + str(ee_pose)) resp = PointResponse() resp.x = ee_pose.pose.position.x resp.y = ee_pose.pose.position.y return resp
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)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) # Initialize the ROS node rospy.init_node('get_joint_states', anonymous=True) # Initialize the MoveIt! commander for the right arm arm = MoveGroupCommander('right_arm') # Get the end-effector link end_effector_link = arm.get_end_effector_link() rospy.loginfo("End effector: %s" % end_effector_link) planning_frame = arm.get_planning_frame() # Joints are stored in the order they appear in the kinematic chain joint_names = arm.get_active_joints() joint_names = [ 'right_arm_shoulder_rotate_joint', 'right_arm_shoulder_lift_joint', 'right_arm_elbow_rotate_joint', 'right_arm_elbow_bend_joint', 'right_arm_wrist_bend_joint', 'right_arm_wrist_rotate_joint' ] # Display the joint names #rospy.loginfo("Joint names:\n" + str(joint_names) + "\n") # Get the current joint angles joint_values = arm.get_current_joint_values() # Display the joint values rospy.loginfo("Joint values:\n" + str(joint_values) + "\n") # Get the end-effector pose ee_pose = arm.get_current_pose(end_effector_link) orientation = ee_pose.pose.orientation ox = orientation.x oy = orientation.y oz = orientation.z ow = orientation.w euler_pose = euler_from_quaternion([ow, ox, oy, oz]) #euler_pose = euler_from_quaternion([0.0, 0.0, 0.0, 1.0]) # Display the end-effector pose rospy.loginfo("End effector pose:\n" + str(ee_pose)) rospy.loginfo("RPY?:\n" + str(euler_pose)) moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def main(): rospy.init_node('test_1', anonymous=True) # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) # Initialize the MoveIt! commander for the right arm right_arm = MoveGroupCommander('tx90_arm') # Get the end-effector link end_effector_link = right_arm.get_end_effector_link() # Get the end-effector pose ee_pose = right_arm.get_current_pose(end_effector_link) r = rospy.Rate(10) while not rospy.is_shutdown(): gripper_pose = arm_utils.tool0TgripperTransform(ee_pose.pose) r.sleep()
def arm_pose(): arm = MoveGroupCommander('arm') arm.allow_replanning(True) end_effector_link = arm.get_end_effector_link() arm.set_goal_position_tolerance(0.03) arm.set_goal_orientation_tolerance(0.025) arm.allow_replanning(True) reference_frame = 'base_footprint' arm.set_pose_reference_frame(reference_frame) arm.set_planning_time(5) curr_pose = arm.get_current_pose(end_effector_link).pose.position return curr_pose.x, curr_pose.y, curr_pose.z
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_cartesian_demo', anonymous=True) # 是否需要使用笛卡尔空间的运动规划 cartesian = rospy.get_param('~cartesian', True) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('arm') 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
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): # 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) # 初始化需要使用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): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('cobra_test_cartesian', anonymous=True) arm = MoveGroupCommander(ARM_GROUP) arm.allow_replanning(True) rospy.sleep(2) pose = PoseStamped().pose # same orientation for all q = quaternion_from_euler(0.0, 0.0, 0.0) # roll, pitch, yaw pose.orientation = Quaternion(*q) i = 1 while i <= 10: waypoints = list() j = 1 while j <= 5: # random pose rand_pose = arm.get_random_pose(arm.get_end_effector_link()) pose.position.x = rand_pose.pose.position.x pose.position.y = rand_pose.pose.position.y pose.position.z = rand_pose.pose.position.z waypoints.append(copy.deepcopy(pose)) j += 1 (plan, fraction) = arm.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold print "====== waypoints created =======" # print waypoints # ======= show cartesian path ====== # arm.go() rospy.sleep(10) i += 1 print "========= end =========" moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_test') arm = MoveGroupCommander('arm') end_effector_link = arm.get_end_effector_link() arm.allow_replanning(True) arm.set_planning_time(5) target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_init_orientation = Quaternion() target_init_orientation = quaternion_from_euler(0.0, 1.57, 0.0) self.setPose(target_pose, [0.5, 0.2, 0.5],list(target_init_orientation)) current_pose = arm.get_current_pose(end_effector_link) self.setPose(current_pose, [current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z], list(target_init_orientation)) arm.set_pose_target(current_pose) arm.go() rospy.sleep(2) constraints = Constraints() orientation_constraint = OrientationConstraint() constraints.name = 'gripper constraint' orientation_constraint.header = target_pose.header orientation_constraint.link_name = end_effector_link orientation_constraint.orientation.x = target_init_orientation[0] orientation_constraint.orientation.y = target_init_orientation[1] orientation_constraint.orientation.z = target_init_orientation[2] orientation_constraint.orientation.w = target_init_orientation[3] orientation_constraint.absolute_x_axis_tolerance = 0.03 orientation_constraint.absolute_y_axis_tolerance = 0.03 orientation_constraint.absolute_z_axis_tolerance = 3.14 orientation_constraint.weight = 1.0 constraints.orientation_constraints.append(orientation_constraint) arm.set_path_constraints(constraints) arm.set_pose_target(target_pose, end_effector_link) arm.go() rospy.sleep(1) moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __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)
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.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.go() rospy.sleep(1) start_pose = arm.get_current_pose(end_effector_link).pose waypoints = [] wpose = deepcopy(start_pose) wpose.position.x -= 0.2 wpose.position.y -= 0.2 #wpose.position.z += 0.6 #wpose.position.x = 0.8 #wpose.position.y = 0.9 #wpose.position.z = 0.9 arm.set_pose_target(wpose) arm.go() rospy.sleep(1) moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('cobra_move_position', anonymous=True) arm = MoveGroupCommander(ARM_GROUP) arm.allow_replanning(True) arm.set_planning_time(10) arm.set_named_target(START_POSITION) arm.go() rospy.sleep(2) print "======== create new goal position ========" start_pose = PoseStamped() start_pose.header.frame_id = arm.get_planning_frame() # Test Position start_pose.pose.position.x = 0.2 # 20 cm start_pose.pose.position.y = -0.11 # -11 cm start_pose.pose.position.z = 0.6 # 60 cm q = quaternion_from_euler(0.0, 0.0, 0.0) start_pose.pose.orientation = Quaternion(*q) print start_pose print "====== move to position =======" # KDL # arm.set_joint_value_target(start_pose, True) # IK Fast arm.set_position_target([start_pose.pose.position.x, start_pose.pose.position.y, start_pose.pose.position.z], arm.get_end_effector_link()) arm.go() rospy.sleep(2) moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): ## Instantiate a `MoveGroupCommander`_ object. This object is an interface ## to a planning group (group of joints). move_group = MoveGroupCommander("panda_arm_hand") move_group_arm = MoveGroupCommander("panda_arm") move_group_arm.set_planning_time(45) ## Instantiate a `RobotCommander`_ object. Provides information such as the robot's ## kinematic model and the robot's current joint states robot = RobotCommander() #end effector link eef_link = move_group_arm.get_end_effector_link() # Misc variables self.robot = robot self.move_group= move_group self.move_group_arm= move_group_arm self.eef_link=eef_link
def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('cobra_test_position', anonymous=True) arm = MoveGroupCommander(ARM_GROUP) arm.allow_replanning(True) # max planning time arm.set_planning_time(10) # start pose arm.set_start_state_to_current_state() end_effector = arm.get_end_effector_link() rospy.sleep(1) print "======== create 100 new goal position ========" start_pose = PoseStamped() start_pose.header.frame_id = arm.get_planning_frame() i = 1 while i <= 1000: # random position start_pose = arm.get_random_pose(end_effector) q = quaternion_from_euler(0.0, 0.0, 0.0) start_pose.pose.orientation = Quaternion(*q) print "====== move to position", i, "=======" # KDL arm.set_joint_value_target(start_pose, True) # IK Fast # arm.set_position_target([start_pose.pose.position.x, start_pose.pose.position.y, # start_pose.pose.position.z], end_effector) i += 1 arm.go() rospy.sleep(2) moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): # 初始化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
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): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) # Initialize the ROS node rospy.init_node('moveit_demo', anonymous=True) robot = RobotCommander() # Connect to the right_arm move group right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Increase the planning time since contraint planning can take a while right_arm.set_planning_time(15) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the right arm reference frame right_arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow some leeway in position(meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.05) right_arm.set_goal_orientation_tolerance(0.1) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Start in the "resting" configuration stored in the SRDF file right_arm.set_named_target('resting') # Plan and execute a trajectory to the goal configuration right_arm.go() rospy.sleep(1) # Open the gripper right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Set an initial target pose with the arm up and to the right target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.237012590198 target_pose.pose.position.y = -0.0747191267505 target_pose.pose.position.z = 0.901578401949 target_pose.pose.orientation.w = 1.0 # Set the start state and target pose, then plan and execute right_arm.set_start_state(robot.get_current_state()) right_arm.set_pose_target(target_pose, end_effector_link) right_arm.go() rospy.sleep(2) # Close the gripper right_gripper.set_joint_value_target(GRIPPER_CLOSED) right_gripper.go() rospy.sleep(1) # Store the current pose start_pose = right_arm.get_current_pose(end_effector_link) # Create a contraints list and give it a name constraints = Constraints() constraints.name = "Keep gripper horizontal" # Create an orientation constraint for the right gripper orientation_constraint = OrientationConstraint() orientation_constraint.header = start_pose.header orientation_constraint.link_name = right_arm.get_end_effector_link() orientation_constraint.orientation.w = 1.0 orientation_constraint.absolute_x_axis_tolerance = 0.1 orientation_constraint.absolute_y_axis_tolerance = 0.1 orientation_constraint.absolute_z_axis_tolerance = 3.14 orientation_constraint.weight = 1.0 # Append the constraint to the list of contraints constraints.orientation_constraints.append(orientation_constraint) # Set the path constraints on the right_arm right_arm.set_path_constraints(constraints) # Set a target pose for the arm target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.173187824708 target_pose.pose.position.y = -0.0159929871606 target_pose.pose.position.z = 0.692596608605 target_pose.pose.orientation.w = 1.0 # Set the start state and target pose, then plan and execute right_arm.set_start_state_to_current_state() right_arm.set_pose_target(target_pose, end_effector_link) right_arm.go() rospy.sleep(1) # Clear all path constraints right_arm.clear_path_constraints() # Open the gripper right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Return to the "resting" configuration stored in the SRDF file right_arm.set_named_target('resting') # Plan and execute a trajectory to the goal configuration right_arm.go() rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit MoveIt moveit_commander.os._exit(0)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) 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) 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) rospy.init_node('moveit_demo') #Initialize robot robot = moveit_commander.RobotCommander() # Use the planning scene object to add or remove objects scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10) # Create a publisher for displaying object frames self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10) # Create a dictionary to hold object colors self.colors = dict() # Initialize the MoveIt! commander for the arm right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the MoveIt! commander for the gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link eef = right_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) # right_arm.set_goal_position_tolerance(0.05) # right_arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the right arm reference frame right_arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt right_arm.set_planning_time(5) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 5 # Set a limit on the number of place attempts max_place_attempts = 5 # Give the scene a chance to catch up rospy.sleep(2) # Prepare Gazebo Subscriber self.pwh = None self.pwh_copy = None self.idx_targ = None self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback) # Prepare Gripper and open it self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction) self.ac.wait_for_server() g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100)) g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.0, 1)) self.ac.send_goal(g_open) rospy.sleep(2) # PREPARE THE SCENE while self.pwh is None: rospy.sleep(0.05) target_id = 'target' self.taid = self.pwh.name.index('wood_cube_5cm') table_id = 'table' self.tid = self.pwh.name.index('table') #obstacle1_id = 'obstacle1' #self.o1id = self.pwh.name.index('wood_block_10_2_1cm') # Remove leftover objects from a previous run scene.remove_world_object(target_id) scene.remove_world_object(table_id) #scene.remove_world_object(obstacle1_id) # Remove any attached objects from a previous session scene.remove_attached_object(GRIPPER_FRAME, target_id) # Set the target size [l, w, h] target_size = [0.05, 0.05, 0.05] table_size = [1.5, 0.8, 0.03] #obstacle1_size = [0.1, 0.025, 0.01] ## Set the target pose on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose = self.pwh.pose[self.taid] target_pose.pose.position.z += 0.025 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose = self.pwh.pose[self.tid] table_pose.pose.position.z += 1 scene.add_box(table_id, table_pose, table_size) #obstacle1_pose = PoseStamped() #obstacle1_pose.header.frame_id = REFERENCE_FRAME #obstacle1_pose.pose = self.pwh.pose[self.o1id] ## Add the target object to the scene #scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size) # Specify a pose to place the target after being picked up place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = 0.50 place_pose.pose.position.y = -0.30 place_pose.pose.orientation.w = 1.0 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) ### Make the target purple ### self.setColor(target_id, 0.6, 0, 1, 1.0) # Send the colors to the planning scene self.sendColors() #print target_pose self.object_frames_pub.publish(target_pose) rospy.sleep(2) # Initialize the grasp pose to the target pose grasp_pose = target_pose #grasp_pose.header.frame_id = 'gazebo_wolrd' # Shift the grasp pose by half the width of the target to center it # grasp_pose.pose.position.y -= target_size[1] / 2.0 # grasp_pose.pose.position.x = target_pose.pose.position.x / 2.0 # grasp_pose.pose.position.x = target_pose.pose.position.x -0.07 # grasp_pose.pose.position.z += 0.18 #Allowed touch object list # ato = [target_id, 'r_forearm_link'] # Generate a list of grasps grasps = self.make_grasps(grasp_pose, [target_id]) #### [target_id] # Publish the grasp poses so they can be viewed in RViz for grasp in grasps: # print grasp.grasp_pose self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) # Track success/failure and number of attempts for pick operation success = False n_attempts = 0 #Allowed touch link list atl = ['r_forearm_link'] # Repeat until we succeed or run out of attempts while success == False and n_attempts < max_pick_attempts: success = right_arm.pick(target_id, grasps) n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) rospy.sleep(0.2) if success: self.ac.send_goal(g_close) rospy.sleep(3) ## If the pick was successful, attempt the place operation #if success: #success = False #n_attempts = 0 ## Generate valid place poses #places = self.make_places(place_pose) ## Repeat until we succeed or run out of attempts #while success == False and n_attempts < max_place_attempts: #for place in places: #success = right_arm.place(target_id, place) #if success: #break #n_attempts += 1 #rospy.loginfo("Place attempt: " + str(n_attempts)) #rospy.sleep(0.2) #if not success: #rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.") #else: #rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.") ## Return the arm to the "resting" pose stored in the SRDF file ##right_arm.set_named_target('resting') ##right_arm.go() ## Open the gripper to the neutral position #right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) #right_gripper.go() #rospy.sleep(1) #rospy.spin() # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('moveit_py_demo', anonymous=True) scene = PlanningSceneInterface() robot = RobotCommander() right_arm = MoveGroupCommander("right_arm") right_gripper = MoveGroupCommander("right_gripper") eef = right_arm.get_end_effector_link() rospy.sleep(2) scene.remove_attached_object("right_gripper_link", "part") # clean the scene scene.remove_world_object("table") scene.remove_world_object("part") right_arm.set_named_target("start1") right_arm.go() right_gripper.set_named_target("open") right_gripper.go() rospy.sleep(1) # publish a demo scene p = PoseStamped() p.header.frame_id = robot.get_planning_frame() # add a table #p.pose.position.x = 0.42 #p.pose.position.y = -0.2 #p.pose.position.z = 0.3 #scene.add_box("table", p, (0.5, 1.5, 0.6)) # add an object to be grasped p.pose.position.x = 0.15 p.pose.position.y = -0.12 p.pose.position.z = 0.7 scene.add_box("part", p, (0.07, 0.01, 0.2)) rospy.sleep(1) g = Grasp() g.id = "test" start_pose = PoseStamped() start_pose.header.frame_id = FIXED_FRAME # start the gripper in a neutral pose part way to the target start_pose.pose.position.x = 0.0130178 start_pose.pose.position.y = -0.125155 start_pose.pose.position.z = 0.597653 start_pose.pose.orientation.x = 0.0 start_pose.pose.orientation.y = 0.388109 start_pose.pose.orientation.z = 0.0 start_pose.pose.orientation.w = 0.921613 right_arm.set_pose_target(start_pose) right_arm.go() rospy.sleep(2) # generate a list of grasps #grasps = self.make_grasps(start_pose) #result = False #n_attempts = 0 # repeat until will succeed #while result == False: #result = robot.right_arm.pick("part", grasps) #n_attempts += 1 #print "Attempts: ", n_attempts #rospy.sleep(0.2) rospy.spin() roscpp_shutdown()
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Construct the initial scene object scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) # Create a dictionary to hold object colors self.colors = dict() # Pause for the scene to get ready rospy.sleep(1) # Initialize the move group for the right arm right_arm = MoveGroupCommander('right_arm') # Initialize the move group for the left arm left_arm = MoveGroupCommander('left_arm') right_arm.set_planner_id("KPIECEkConfigDefault"); left_arm.set_planner_id("KPIECEkConfigDefault"); rospy.sleep(1) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.01) right_arm.set_goal_orientation_tolerance(0.05) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the reference frame for pose targets reference_frame = 'base_footprint' # Set the right arm reference frame accordingly right_arm.set_pose_reference_frame(reference_frame) # Allow 5 seconds per planning attempt right_arm.set_planning_time(5) # Give each of the scene objects a unique name table_id = 'table' box1_id = 'box1' box2_id = 'box2' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "resting" pose stored in the SRDF file #left_arm.set_named_target('left_start') #left_arm.go() # Start the arm in the "resting" pose stored in the SRDF file right_arm.set_named_target('right_start') right_arm.go() rospy.sleep(2) # Set the height of the table off the ground table_ground = 0.75 # Set the length, width and height of the table and boxes table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # Add a table top and two boxes to the scene table_pose = PoseStamped() table_pose.header.frame_id = reference_frame table_pose.pose.position.x = 0.56 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose = PoseStamped() box1_pose.header.frame_id = reference_frame box1_pose.pose.position.x = 0.51 box1_pose.pose.position.y = -0.1 box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0 box1_pose.pose.orientation.w = 1.0 scene.add_box(box1_id, box1_pose, box1_size) box2_pose = PoseStamped() box2_pose.header.frame_id = reference_frame box2_pose.pose.position.x = 0.49 box2_pose.pose.position.y = 0.15 box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 scene.add_box(box2_id, box2_pose, box2_size) # Make the table red and the boxes orange self.setColor(table_id, 0.8, 0, 0, 1.0) self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the target pose in between the boxes and above the table target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.pose.position.x = 0.4 target_pose.pose.position.y = 0.0 target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05 target_pose.pose.orientation.w = 1.0 # Set the target pose for the arm right_arm.set_pose_target(target_pose, end_effector_link) # Move the arm to the target pose (if possible) right_arm.go() # Pause for a moment... rospy.sleep(2) # Return the arm to the "resting" pose stored in the SRDF file right_arm.set_named_target('right_start') right_arm.go() # Exit MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('cobra_move_cartesian', anonymous=True) arm = MoveGroupCommander(ARM_GROUP) arm.allow_replanning(True) # arm.set_goal_position_tolerance(0.01) arm.set_named_target(START_POSITION) arm.go() rospy.sleep(2) waypoints = list() pose = PoseStamped().pose # start with the current pose waypoints.append(copy.deepcopy(arm.get_current_pose(arm.get_end_effector_link()).pose)) # same orientation for all q = quaternion_from_euler(0.0, 0.0, 0.0) # roll, pitch, yaw pose.orientation = Quaternion(*q) # first pose pose.position.x = 0.35 pose.position.y = 0.25 pose.position.z = 0.3 waypoints.append(copy.deepcopy(pose)) # second pose pose.position.x = 0.25 pose.position.y = -0.3 pose.position.z = 0.3 waypoints.append(copy.deepcopy(pose)) # third pose pose.position.x += 0.15 waypoints.append(copy.deepcopy(pose)) # fourth pose pose.position.y += 0.15 waypoints.append(copy.deepcopy(pose)) (plan, fraction) = arm.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold print "====== waypoints created =======" # print waypoints # ======= show cartesian path ====== # arm.go() rospy.sleep(10) print "========= end =========" moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('moveit_py_demo', anonymous=True) scene = PlanningSceneInterface() robot = RobotCommander() right_arm = MoveGroupCommander(GROUP_NAME_ARM) #right_arm.set_planner_id("KPIECEkConfigDefault"); right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) eef = right_arm.get_end_effector_link() rospy.sleep(2) scene.remove_attached_object(GRIPPER_FRAME, "part") # clean the scene scene.remove_world_object("table") scene.remove_world_object("part") #right_arm.set_named_target("r_start") #right_arm.go() #right_gripper.set_named_target("right_gripper_open") #right_gripper.go() rospy.sleep(1) # publish a demo scene p = PoseStamped() p.header.frame_id = robot.get_planning_frame() # add a table #p.pose.position.x = 0.42 #p.pose.position.y = -0.2 #p.pose.position.z = 0.3 #scene.add_box("table", p, (0.5, 1.5, 0.6)) # add an object to be grasped p.pose.position.x = 0.7 p.pose.position.y = -0.2 p.pose.position.z = 0.85 scene.add_box("part", p, (0.07, 0.01, 0.2)) rospy.sleep(1) g = Grasp() g.id = "test" start_pose = PoseStamped() start_pose.header.frame_id = FIXED_FRAME # start the gripper in a neutral pose part way to the target start_pose.pose.position.x = 0.47636 start_pose.pose.position.y = -0.21886 start_pose.pose.position.z = 0.9 start_pose.pose.orientation.x = 0.00080331 start_pose.pose.orientation.y = 0.001589 start_pose.pose.orientation.z = -2.4165e-06 start_pose.pose.orientation.w = 1 right_arm.set_pose_target(start_pose) right_arm.go() rospy.sleep(2) # generate a list of grasps grasps = self.make_grasps(start_pose) result = False n_attempts = 0 # repeat until will succeed while result == False: result = robot.right_arm.pick("part", grasps) n_attempts += 1 print "Attempts: ", n_attempts rospy.sleep(0.3) rospy.spin() roscpp_shutdown()
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)
if __name__=='__main__': roscpp_initialize(sys.argv) rospy.init_node('moveit_py_demo', anonymous=True) scene = PlanningSceneInterface() robot = RobotCommander() right_arm = MoveGroupCommander("arm") print "============ Robot Groups:" print robot.get_group_names() print "============ Robot Links for arm:" print robot.get_link_names("arm") print "============ Robot Links for gripper:" print robot.get_link_names("gripper") print right_arm.get_end_effector_link() print "============ Printing robot state" #print robot.get_current_state() print "============" rospy.sleep(1) # clean the scene scene.remove_world_object("pole") scene.remove_world_object("table") #scene.remove_world_object("part") scene.remove_world_object("grasp_marker") # publish a demo scene p = PoseStamped() p.header.frame_id = robot.get_planning_frame()
# arm.set_goal_tolerance(0.01) # Action Clients for Place rospy.loginfo("Connecting to place AS") place_ac = actionlib.SimpleActionClient('/place', PlaceAction) place_ac.wait_for_server() rospy.loginfo("Successfully connected") rospy.sleep(1) # take the part to specific pose pose = generate_rand_position() # IKFast arm.set_position_target([pose.pose.position.x, pose.pose.position.y, pose.pose.position.z], arm.get_end_effector_link()) # KDL # arm.set_joint_value_target(pose, True) arm.go() rospy.sleep(5) # ===== place start ==== # place_result = place(PLANNING_GROUP, PICK_OBJECT, generate_place_pose()) rospy.loginfo("Place Result is:") rospy.loginfo("Human readable error: " + str(moveit_error_dict[place_result.error_code.val])) rospy.sleep(5) # end moveit_commander.roscpp_shutdown()
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)
if __name__=='__main__': roscpp_initialize(sys.argv) rospy.init_node('moveit_py_demo', anonymous=True) scene = PlanningSceneInterface() robot = RobotCommander() group = MoveGroupCommander("arm") print "============ Robot Groups:" print robot.get_group_names() print "============ Robot Links for arm:" print robot.get_link_names("arm") print "============ Robot Links for gripper:" print robot.get_link_names("gripper") print group.get_end_effector_link() print group.get_pose_reference_frame() print "============ Printing robot state" #print robot.get_current_state() print "============" tl = tf.TransformListener() rospy.sleep(1) waypoints = [] # start with the current pose waypoints.append(group.get_current_pose().pose) print waypoints[0] currentPose = PoseStamped() currentPose.header.frame_id = group.get_pose_reference_frame()
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)
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)
pose_grasp = copy.deepcopy(p) # create list of grasps possible_grasps = create_random_grasps(pose_grasp.pose, 100) # create goal for pick goal = create_pickup_goal(PLANNING_GROUP, PICK_OBJECT, pose_grasp, possible_grasps) rospy.loginfo("Sending goal") pickup_ac.send_goal(goal) rospy.loginfo("Waiting for result") pickup_ac.wait_for_result() result = pickup_ac.get_result() rospy.loginfo("Pick Result is:") rospy.loginfo("Human readable error: " + str(moveit_error_dict[result.error_code.val])) rospy.sleep(5) # clean world rospy.loginfo("Cleaning world objects") scene.remove_world_object(TABLE_OBJECT) scene.remove_world_object(PICK_OBJECT) scene.remove_attached_object(arm.get_end_effector_link(), PICK_OBJECT) rospy.sleep(2) i += 1 # end moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Use the planning scene object to add or remove objects scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped) # Create a dictionary to hold object colors self.colors = dict() # Initialize the move group for the right arm right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.05) right_arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the right arm reference frame right_arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 10 seconds per planning attempt right_arm.set_planning_time(10) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 10 # Set a limit on the number of place attempts max_place_attempts = 5 # Give the scene a chance to catch up rospy.sleep(2) # Give each of the scene objects a unique name table_id = 'table' box1_id = 'box1' box2_id = 'box2' target_id = 'target' tool_id = 'tool' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_world_object(target_id) scene.remove_world_object(tool_id) # Remove any attached objects from a previous session scene.remove_attached_object(GRIPPER_FRAME, target_id) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "resting" pose stored in the SRDF file right_arm.set_named_target('right_start') right_arm.go() # Open the gripper to the neutral position right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Set the height of the table off the ground table_ground = 0.65 # Set the dimensions of the scene objects [l, w, h] table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # Set the target size [l, w, h] target_size = [0.02, 0.01, 0.12] # Add a table top and two boxes to the scene table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose.position.x = 0.55 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose = PoseStamped() box1_pose.header.frame_id = REFERENCE_FRAME box1_pose.pose.position.x = 0.55 box1_pose.pose.position.y = -0.1 box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0 box1_pose.pose.orientation.w = 1.0 scene.add_box(box1_id, box1_pose, box1_size) box2_pose = PoseStamped() box2_pose.header.frame_id = REFERENCE_FRAME box2_pose.pose.position.x = 0.54 box2_pose.pose.position.y = 0.13 box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 scene.add_box(box2_id, box2_pose, box2_size) # Set the target pose in between the boxes and on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.60 target_pose.pose.position.y = 0.0 target_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0 target_pose.pose.orientation.w = 1.0 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) # Make the table red and the boxes orange self.setColor(table_id, 0.8, 0, 0, 1.0) self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # Make the target yellow self.setColor(target_id, 0.9, 0.9, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the support surface name to the table object right_arm.set_support_surface_name(table_id) # Specify a pose to place the target after being picked up place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = 0.50 place_pose.pose.position.y = -0.25 place_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0 place_pose.pose.orientation.w = 1.0 # Initialize the grasp pose to the target pose grasp_pose = target_pose # Shift the grasp pose by half the width of the target to center it grasp_pose.pose.position.y -= target_size[1] / 2.0 # Generate a list of grasps grasps = self.make_grasps(grasp_pose, [target_id]) # Publish the grasp poses so they can be viewed in RViz for grasp in grasps: self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) # Track success/failure and number of attempts for pick operation result = None n_attempts = 0 # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) result = right_arm.pick(target_id, grasps) rospy.sleep(0.2) # If the pick was successful, attempt the place operation if result == MoveItErrorCodes.SUCCESS: result = None n_attempts = 0 #_------------------------now we move to the other table__________------------------------------------------- #_------------------------now we move to the other table__________------------------------------------------- # Generate valid place poses places = self.make_places(place_pose) # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts: n_attempts += 1 rospy.loginfo("Place attempt: " + str(n_attempts)) for place in places: result = right_arm.place(target_id, place) if result == MoveItErrorCodes.SUCCESS: break rospy.sleep(0.2) if result != MoveItErrorCodes.SUCCESS: rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.") else: rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.") # Return the arm to the "resting" pose stored in the SRDF file right_arm.set_named_target('right_start') right_arm.go() # Open the gripper to the neutral position right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
class SmartGrasper(object): """ This is the helper library to easily access the different functionalities of the simulated robot from python. """ __last_joint_state = None __current_model_name = "cricket_ball" __path_to_models = "/root/.gazebo/models/" def __init__(self): """ This constructor initialises the different necessary connections to the topics and services and resets the world to start in a good position. """ rospy.init_node("smart_grasper") self.__joint_state_sub = rospy.Subscriber("/joint_states", JointState, self.__joint_state_cb, queue_size=1) rospy.wait_for_service("/gazebo/get_model_state", 10.0) rospy.wait_for_service("/gazebo/reset_world", 10.0) self.__reset_world = rospy.ServiceProxy("/gazebo/reset_world", Empty) self.__get_pose_srv = rospy.ServiceProxy("/gazebo/get_model_state", GetModelState) rospy.wait_for_service("/gazebo/pause_physics") self.__pause_physics = rospy.ServiceProxy("/gazebo/pause_physics", Empty) rospy.wait_for_service("/gazebo/unpause_physics") self.__unpause_physics = rospy.ServiceProxy("/gazebo/unpause_physics", Empty) rospy.wait_for_service("/controller_manager/switch_controller") self.__switch_ctrl = rospy.ServiceProxy("/controller_manager/switch_controller", SwitchController) rospy.wait_for_service("/gazebo/set_model_configuration") self.__set_model = rospy.ServiceProxy("/gazebo/set_model_configuration", SetModelConfiguration) rospy.wait_for_service("/gazebo/delete_model") self.__delete_model = rospy.ServiceProxy("/gazebo/delete_model", DeleteModel) rospy.wait_for_service("/gazebo/spawn_sdf_model") self.__spawn_model = rospy.ServiceProxy("/gazebo/spawn_sdf_model", SpawnModel) rospy.wait_for_service('/get_planning_scene', 10.0) self.__get_planning_scene = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene) self.__pub_planning_scene = rospy.Publisher('/planning_scene', PlanningScene, queue_size=10, latch=True) self.arm_commander = MoveGroupCommander("arm") self.hand_commander = MoveGroupCommander("hand") self.__hand_traj_client = SimpleActionClient("/hand_controller/follow_joint_trajectory", FollowJointTrajectoryAction) self.__arm_traj_client = SimpleActionClient("/arm_controller/follow_joint_trajectory", FollowJointTrajectoryAction) if self.__hand_traj_client.wait_for_server(timeout=rospy.Duration(4.0)) is False: rospy.logfatal("Failed to connect to /hand_controller/follow_joint_trajectory in 4sec.") raise Exception("Failed to connect to /hand_controller/follow_joint_trajectory in 4sec.") if self.__arm_traj_client.wait_for_server(timeout=rospy.Duration(4.0)) is False: rospy.logfatal("Failed to connect to /arm_controller/follow_joint_trajectory in 4sec.") raise Exception("Failed to connect to /arm_controller/follow_joint_trajectory in 4sec.") self.reset_world() def reset_world(self): """ Resets the object poses in the world and the robot joint angles. """ self.__switch_ctrl.call(start_controllers=[], stop_controllers=["hand_controller", "arm_controller", "joint_state_controller"], strictness=SwitchControllerRequest.BEST_EFFORT) self.__pause_physics.call() joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint', 'H1_F1J1', 'H1_F1J2', 'H1_F1J3', 'H1_F2J1', 'H1_F2J2', 'H1_F2J3', 'H1_F3J1', 'H1_F3J2', 'H1_F3J3'] joint_positions = [1.2, 0.3, -1.5, -0.5, -1.5, 0.0, 0.0, -0.3, 0.0, 0.0, -0.3, 0.0, 0.0, -0.3, 0.0] self.__set_model.call(model_name="smart_grasping_sandbox", urdf_param_name="robot_description", joint_names=joint_names, joint_positions=joint_positions) timer = Timer(0.0, self.__start_ctrl) timer.start() time.sleep(0.1) self.__unpause_physics.call() self.__reset_world.call() def get_object_pose(self): """ Gets the pose of the ball in the world frame. @return The pose of the ball. """ return self.__get_pose_srv.call(self.__current_model_name, "world").pose def get_tip_pose(self): """ Gets the current pose of the robot's tooltip in the world frame. @return the tip pose """ return self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose def move_tip_absolute(self, target): """ Moves the tooltip to the absolute target in the world frame @param target is a geometry_msgs.msg.Pose @return True on success """ self.arm_commander.set_start_state_to_current_state() self.arm_commander.set_pose_targets([target]) plan = self.arm_commander.plan() if not self.arm_commander.execute(plan): return False return True def move_tip(self, x=0., y=0., z=0., roll=0., pitch=0., yaw=0.): """ Moves the tooltip in the world frame by the given x,y,z / roll,pitch,yaw. @return True on success """ transform = PyKDL.Frame(PyKDL.Rotation.RPY(pitch, roll, yaw), PyKDL.Vector(-x, -y, -z)) tip_pose = self.get_tip_pose() tip_pose_kdl = posemath.fromMsg(tip_pose) final_pose = toMsg(tip_pose_kdl * transform) self.arm_commander.set_start_state_to_current_state() self.arm_commander.set_pose_targets([final_pose]) plan = self.arm_commander.plan() if not self.arm_commander.execute(plan): return False return True def send_command(self, command, duration=0.2): """ Send a dictionnary of joint targets to the arm and hand directly. @param command: a dictionnary of joint names associated with a target: {"H1_F1J1": -1.0, "shoulder_pan_joint": 1.0} @param duration: the amount of time it will take to get there in seconds. Needs to be bigger than 0.0 """ hand_goal = None arm_goal = None for joint, target in command.items(): if "H1" in joint: if not hand_goal: hand_goal = FollowJointTrajectoryGoal() point = JointTrajectoryPoint() point.time_from_start = rospy.Duration.from_sec(duration) hand_goal.trajectory.points.append(point) hand_goal.trajectory.joint_names.append(joint) hand_goal.trajectory.points[0].positions.append(target) else: if not arm_goal: arm_goal = FollowJointTrajectoryGoal() point = JointTrajectoryPoint() point.time_from_start = rospy.Duration.from_sec(duration) arm_goal.trajectory.points.append(point) arm_goal.trajectory.joint_names.append(joint) arm_goal.trajectory.points[0].positions.append(target) if arm_goal: self.__arm_traj_client.send_goal_and_wait(arm_goal) if hand_goal: self.__hand_traj_client.send_goal_and_wait(hand_goal) def get_current_joint_state(self): """ Gets the current state of the robot. @return joint positions, velocity and efforts as three dictionnaries """ joints_position = {n: p for n, p in zip(self.__last_joint_state.name, self.__last_joint_state.position)} joints_velocity = {n: v for n, v in zip(self.__last_joint_state.name, self.__last_joint_state.velocity)} joints_effort = {n: v for n, v in zip(self.__last_joint_state.name, self.__last_joint_state.effort)} return joints_position, joints_velocity, joints_effort def open_hand(self): """ Opens the hand. @return True on success """ self.hand_commander.set_named_target("open") plan = self.hand_commander.plan() if not self.hand_commander.execute(plan, wait=True): return False return True def close_hand(self): """ Closes the hand. @return True on success """ self.hand_commander.set_named_target("close") plan = self.hand_commander.plan() if not self.hand_commander.execute(plan, wait=True): return False return True def check_fingers_collisions(self, enable=True): """ Disables or enables the collisions check between the fingers and the objects / table @param enable: set to True to enable / False to disable @return True on success """ objects = ["cricket_ball__link", "drill__link", "cafe_table__link"] while self.__pub_planning_scene.get_num_connections() < 1: rospy.loginfo("waiting for someone to subscribe to the /planning_scene") rospy.sleep(0.1) request = PlanningSceneComponents(components=PlanningSceneComponents.ALLOWED_COLLISION_MATRIX) response = self.__get_planning_scene(request) acm = response.scene.allowed_collision_matrix for object_name in objects: if object_name not in acm.entry_names: # add object to allowed collision matrix acm.entry_names += [object_name] for row in range(len(acm.entry_values)): acm.entry_values[row].enabled += [False] new_row = deepcopy(acm.entry_values[0]) acm.entry_values += {new_row} for index_entry_values, entry_values in enumerate(acm.entry_values): if "H1_F" in acm.entry_names[index_entry_values]: for index_value, _ in enumerate(entry_values.enabled): if acm.entry_names[index_value] in objects: if enable: acm.entry_values[index_entry_values].enabled[index_value] = False else: acm.entry_values[index_entry_values].enabled[index_value] = True elif acm.entry_names[index_entry_values] in objects: for index_value, _ in enumerate(entry_values.enabled): if "H1_F" in acm.entry_names[index_value]: if enable: acm.entry_values[index_entry_values].enabled[index_value] = False else: acm.entry_values[index_entry_values].enabled[index_value] = True planning_scene_diff = PlanningScene(is_diff=True, allowed_collision_matrix=acm) self.__pub_planning_scene.publish(planning_scene_diff) rospy.sleep(1.0) return True def pick(self): """ Does its best to pick the ball. """ rospy.loginfo("Moving to Pregrasp") self.open_hand() time.sleep(0.1) ball_pose = self.get_object_pose() ball_pose.position.z += 0.5 #setting an absolute orientation (from the top) quaternion = quaternion_from_euler(-pi/2., 0.0, 0.0) ball_pose.orientation.x = quaternion[0] ball_pose.orientation.y = quaternion[1] ball_pose.orientation.z = quaternion[2] ball_pose.orientation.w = quaternion[3] self.move_tip_absolute(ball_pose) time.sleep(0.1) rospy.loginfo("Grasping") self.move_tip(y=-0.164) time.sleep(0.1) self.check_fingers_collisions(False) time.sleep(0.1) self.close_hand() time.sleep(0.1) rospy.loginfo("Lifting") for _ in range(5): self.move_tip(y=0.01) time.sleep(0.1) self.check_fingers_collisions(True) def swap_object(self, new_model_name): """ Replaces the current object with a new one.Replaces @new_model_name the name of the folder in which the object is (e.g. beer) """ try: self.__delete_model(self.__current_model_name) except: rospy.logwarn("Failed to delete: " + self.__current_model_name) try: sdf = None initial_pose = Pose() initial_pose.position.x = 0.15 initial_pose.position.z = 0.82 with open(self.__path_to_models + new_model_name + "/model.sdf", "r") as model: sdf = model.read() res = self.__spawn_model(new_model_name, sdf, "", initial_pose, "world") rospy.logerr( "RES: " + str(res) ) self.__current_model_name = new_model_name except: rospy.logwarn("Failed to delete: " + self.__current_model_name) def __compute_arm_target_for_ball(self): ball_pose = self.get_object_pose() # come at it from the top arm_target = ball_pose arm_target.position.z += 0.5 quaternion = quaternion_from_euler(-pi/2., 0.0, 0.0) arm_target.orientation.x = quaternion[0] arm_target.orientation.y = quaternion[1] arm_target.orientation.z = quaternion[2] arm_target.orientation.w = quaternion[3] return arm_target def __pre_grasp(self, arm_target): self.hand_commander.set_named_target("open") plan = self.hand_commander.plan() self.hand_commander.execute(plan, wait=True) for _ in range(10): self.arm_commander.set_start_state_to_current_state() self.arm_commander.set_pose_targets([arm_target]) plan = self.arm_commander.plan() if self.arm_commander.execute(plan): return True def __grasp(self, arm_target): waypoints = [] waypoints.append(self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose) arm_above_ball = deepcopy(arm_target) arm_above_ball.position.z -= 0.12 waypoints.append(arm_above_ball) self.arm_commander.set_start_state_to_current_state() (plan, fraction) = self.arm_commander.compute_cartesian_path(waypoints, 0.01, 0.0) print fraction if not self.arm_commander.execute(plan): return False self.hand_commander.set_named_target("close") plan = self.hand_commander.plan() if not self.hand_commander.execute(plan, wait=True): return False self.hand_commander.attach_object("cricket_ball__link") def __lift(self, arm_target): waypoints = [] waypoints.append(self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose) arm_above_ball = deepcopy(arm_target) arm_above_ball.position.z += 0.1 waypoints.append(arm_above_ball) self.arm_commander.set_start_state_to_current_state() (plan, fraction) = self.arm_commander.compute_cartesian_path(waypoints, 0.01, 0.0) print fraction if not self.arm_commander.execute(plan): return False def __start_ctrl(self): rospy.loginfo("STARTING CONTROLLERS") self.__switch_ctrl.call(start_controllers=["hand_controller", "arm_controller", "joint_state_controller"], stop_controllers=[], strictness=1) def __joint_state_cb(self, msg): self.__last_joint_state = msg
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)
p.header.frame_id = REFERENCE_LINK p.header.stamp = rospy.Time.now() quat = quaternion_from_euler(0.0, 0.0, 0.0) # roll, pitch, yaw p.pose.orientation = Quaternion(*quat) p.pose.position.x = 0.6 p.pose.position.y = 0.0 p.pose.position.z = 0.0 # add table scene.add_box(TABLE_OBJECT, p, (0.5, 0.5, 0.2)) i = 1 while i <= 10: gripper_pose = arm.get_current_pose(arm.get_end_effector_link()) # part p.pose.position.x = gripper_pose.pose.position.x p.pose.position.y = gripper_pose.pose.position.y p.pose.position.z = gripper_pose.pose.position.z # add part scene.add_box(PICK_OBJECT, p, (0.07, 0.07, 0.1)) rospy.loginfo("Added object to world") # attach object manually arm.attach_object(PICK_OBJECT, arm.get_end_effector_link(), GRIPPER_JOINTS) rospy.sleep(1) # ===== place start ==== # place_result = place(PLANNING_GROUP, PICK_OBJECT, generate_place_pose())
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) 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)