def InitializeMoveitCommander(): #First initialize moveit_commander print "============ Starting tutorial setup" moveit_commander.roscpp_initialize(sys.argv) #Instantiate a RobotCommander object. This object is an interface to the robot as a whole. robot = moveit_commander.RobotCommander() #Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up. print "============ Waiting for RVIZ..." rospy.sleep(1) print "============ Starting tutorial " #Instantiate a PlanningSceneInterface object. This object is an interface to the world surrounding the robot. scene = moveit_commander.PlanningSceneInterface() #Instantiate a MoveGroupCommander object. This object is an interface to one group of joints. In this case the group is the joints in the left arm. This interface can be used to plan and execute motions on the left arm. global group_both_arms, group_left_arm, group_right_arm group_both_arms = MoveGroupCommander("both_arms") group_both_arms.set_goal_position_tolerance(0.01) group_both_arms.set_goal_orientation_tolerance(0.01) group_left_arm = MoveGroupCommander("left_arm") group_left_arm.set_goal_position_tolerance(0.01) group_left_arm.set_goal_orientation_tolerance(0.01) group_right_arm = MoveGroupCommander("right_arm") group_right_arm.set_goal_position_tolerance(0.01) group_right_arm.set_goal_orientation_tolerance(0.01) #We create this DisplayTrajectory publisher which is used below to publish trajectories for RVIZ to visualize. display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory) # Obtain current poses of left and right end-effectors: [x,y,z,roll,pitch,yaw].T P_left_pose = group_left_arm.get_current_pose() P_right_pose = group_right_arm.get_current_pose() P_left_euler = group_left_arm.get_current_rpy() P_right_euler = group_right_arm.get_current_rpy() global P_left_current, P_right_current P_left_current = np.array([[P_left_pose.pose.position.x], [P_left_pose.pose.position.y], [P_left_pose.pose.position.z], [P_left_euler[0]], [P_left_euler[1]], [P_left_euler[2]]]) P_right_current = np.array([[P_right_pose.pose.position.x], [P_right_pose.pose.position.y], [P_right_pose.pose.position.z], [P_right_euler[0]], [P_right_euler[1]], [P_right_euler[1]]]) print "\nCurrent pose of left and right EE: \n", np.transpose( P_left_current), "\n", np.transpose(P_right_current)
def __init__(self): group = MoveGroupCommander("arm") #group.set_orientation_tolerance([0.3,0.3,0,3]) p = PoseStamped() p.header.frame_id = "/katana_base_link" p.pose.position.x = 0.4287 #p.pose.position.y = -0.0847 p.pose.position.y = 0.0 p.pose.position.z = 0.4492 q = quaternion_from_euler(2, 0, 0) p.pose.orientation.x = q[0] p.pose.orientation.y = q[1] p.pose.orientation.z = q[2] p.pose.orientation.w = q[3] print "Planning frame: ", group.get_planning_frame() print "Pose reference frame: ", group.get_pose_reference_frame() #group.set_pose_reference_frame("katana_base_link") print "RPy target: 0,0,0" #group.set_rpy_target([0, 0, 0],"katana_gripper_tool_frame") #group.set_position_target([0.16,0,0.40], "katana_gripper_tool_frame") group.set_pose_target(p, "katana_gripper_tool_frame") group.go() print "Current rpy: ", group.get_current_rpy( "katana_gripper_tool_frame")
def __init__(self): group = MoveGroupCommander("arm") #group.set_orientation_tolerance([0.3,0.3,0,3]) p = PoseStamped() p.header.frame_id = "/katana_base_link" p.pose.position.x = 0.4287 #p.pose.position.y = -0.0847 p.pose.position.y = 0.0 p.pose.position.z = 0.4492 q = quaternion_from_euler(2, 0, 0) p.pose.orientation.x = q[0] p.pose.orientation.y = q[1] p.pose.orientation.z = q[2] p.pose.orientation.w = q[3] print "Planning frame: " ,group.get_planning_frame() print "Pose reference frame: ",group.get_pose_reference_frame() #group.set_pose_reference_frame("katana_base_link") print "RPy target: 0,0,0" #group.set_rpy_target([0, 0, 0],"katana_gripper_tool_frame") #group.set_position_target([0.16,0,0.40], "katana_gripper_tool_frame") group.set_pose_target(p, "katana_gripper_tool_frame") group.go() print "Current rpy: " , group.get_current_rpy("katana_gripper_tool_frame")
raw_input("please input") # [00000000] print right_arm.get_joint_value_target() print both_arms.get_joint_value_target() # no this functions # print right_arm.get_named_targets() print right_arm.get_remembered_joint_values() print both_arms.get_remembered_joint_values() print right_arm.get_path_constraints() print both_arms.get_path_constraints() print right_arm.get_active_joints() print both_arms.get_active_joints() print right_arm.get_current_joint_values() print right_arm.get_current_pose() print right_arm.get_current_rpy() print both_arms.get_current_joint_values() print both_arms.get_current_pose() print both_arms.get_current_rpy() right_arm.clear_pose_targets() left_current_pose = both_arms.get_current_pose(end_effector_link='left_gripper').pose right_current_pose = both_arms.get_current_pose(end_effector_link='right_gripper').pose print left_current_pose
group = MoveGroupCommander("arm") gripper_group = MoveGroupCommander("gripper") group.set_planning_time( 600.0 ) # Getting Initial Pose & RPY pose_init = group.get_current_pose() rospy.loginfo( "Get Initial Pose\n{}".format( pose_init ) ) # add gripper joint status gripper_status_init = gripper_group.get_current_joint_values() rospy.loginfo( "Get Initial Posesssssss\n{}".format(gripper_status_init) ) rpy_init = group.get_current_rpy() rospy.loginfo( "Get Initial RPY:{}".format( rpy_init ) ) # Pose 1 rospy.loginfo( "Starting Pose 1") pose_target_1 = [ 0.12, 0.0, 0.1, 0.0, math.pi/2.0, 0.0 ] # [ x, y, z, r, p, y ] group.set_pose_target( pose_target_1 ) group.go() gripper_target_1 = [ 0.01, 0] gripper_group.set_joint_value_target(gripper_target_1) gripper_group.go()
from moveit_msgs.msg import RobotTrajectory, Grasp if __name__ == '__main__': rospy.init_node('moveit_py_demo', anonymous=True) scene = PlanningSceneInterface() robot = MoveGroupCommander("arm_1") rospy.sleep(1) currentrf = robot.get_pose_reference_frame() robot.set_pose_reference_frame(currentrf) newpose = robot.get_current_pose().pose a = robot.get_current_pose().pose newposerpy = robot.get_current_rpy() newpose.position.x = newpose.position.x - delta_x newpose.position.x = newpose.position.y - delta_y newpose.position.x = newpose.position.z - delta_z robot.set_pose_target(newpose) planned = robot.plan() robot.execute(planned) rospy.sleep(2) newpose = robot.get_current_pose().pose robot.set_start_state_to_current_state() robot.set_pose_target(a) b = robot.compute_cartesian_path([newpose, a], 0.01, 10000)