def move_robot(): print "Starting rospy and moveit..." moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('move_group_py') robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() group1 = moveit_commander.MoveGroupCommander("both_arms") display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',moveit_msgs.msg.DisplayTrajectory, queue_size=1) group1.allow_replanning(True) group1.set_planning_time(10.0) print "Waiting for RViz" rospy.sleep(10) print "Initializing..." print "Reference frame: %s" %group1.get_planning_frame() print "Generating plan..." group_values = group1.get_current_joint_values() group_values[1] = 0 group_values[2] = 0 group1.set_joint_value_target(group_values) plan1 = group1.plan() rospy.sleep(5) print "Executing..." plan1 = group1.go()
def starting_pose(): rospy.sleep(20) moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('move_to_starting_pose', anonymous=True) robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() group = moveit_commander.MoveGroupCommander("arm_gp") display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory) print "============ Waiting for RVIZ..." pose_target = geometry_msgs.msg.Pose() pose_target.orientation.x = 0.0 pose_target.orientation.y = -1.0 pose_target.orientation.z = 0.0 pose_target.orientation.w = 0.0 pose_target.position.x = 0.3 pose_target.position.y = 0.0 pose_target.position.z = 0.38 group.set_pose_target(pose_target) group.go(wait=True) ## When finished shut down moveit_commander. moveit_commander.roscpp_shutdown()
def init(): global robot global scene global left_arm global right_arm global left_gripper global right_gripper robot = None scene = None left_arm = None right_arm = None left_gripper = None right_gripper = None gc.collect() #Initialize moveit_commander moveit_commander.roscpp_initialize(sys.argv) #Initialize both arms robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() left_arm = moveit_commander.MoveGroupCommander('left_arm') right_arm = moveit_commander.MoveGroupCommander('right_arm') left_arm.set_planner_id('RRTConnectkConfigDefault') left_arm.set_planning_time(20) right_arm.set_planner_id('RRTConnectkConfigDefault') right_arm.set_planning_time(20)
def __init__(self): self.state = "INITIALIZING" moveit_commander.roscpp_initialize(sys.argv) #Start a node rospy.init_node('moveit_node') #Initialize subscriber self.sub = rospy.Subscriber("game_state", String, self.state_machine) #Initialize both arms self.robot = moveit_commander.RobotCommander() self.scene = moveit_commander.PlanningSceneInterface() self.left_arm = moveit_commander.MoveGroupCommander('left_arm') self.right_arm = moveit_commander.MoveGroupCommander('right_arm') self.left_arm.set_planner_id('RRTConnectkConfigDefault') self.left_arm.set_planning_time(10) self.right_arm.set_planner_id('RRTConnectkConfigDefault') self.right_arm.set_planning_time(10) #Set up the grippers self.left_gripper = baxter_gripper.Gripper('left') self.right_gripper = baxter_gripper.Gripper('right') #Calibrate the gripper (other commands won't work unless you do this first) print('Calibrating...') self.right_gripper.calibrate() rospy.sleep(0.5) #Initialize class variables self.all_pieces = [0,1,2,3,4,5,6,7,8,9,13,14,15,16,20] self.available_pieces = self.all_pieces self.state = "PLAY"
def setUp(self): # create a action client of move group self._move_client = SimpleActionClient('move_group', MoveGroupAction) self._move_client.wait_for_server() moveit_commander.roscpp_initialize(sys.argv) group_name = moveit_commander.RobotCommander().get_group_names()[0] group = moveit_commander.MoveGroupCommander(group_name) # prepare a joint goal self._goal = MoveGroupGoal() self._goal.request.group_name = group_name self._goal.request.max_velocity_scaling_factor = 0.1 self._goal.request.max_acceleration_scaling_factor = 0.1 self._goal.request.start_state.is_diff = True goal_constraint = Constraints() joint_values = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6] joint_names = group.get_active_joints() for i in range(len(joint_names)): joint_constraint = JointConstraint() joint_constraint.joint_name = joint_names[i] joint_constraint.position = joint_values[i] joint_constraint.weight = 1.0 goal_constraint.joint_constraints.append(joint_constraint) self._goal.request.goal_constraints.append(goal_constraint) self._goal.planning_options.planning_scene_diff.robot_state.is_diff = True
def followCartTrajMoveit(self, x_vec, dt): moveit_commander.roscpp_initialize(sys.argv) group = moveit_commander.MoveGroupCommander("left_arm") waypoints = [] waypoints.append(group.get_current_pose().pose) for i in range(len(x_vec)): #Make sure quaternion is normalized x_vec[i][3:7] = x_vec[i][3:7] / la.norm(x_vec[i][3:7]) wpose = geometry_msgs.msg.Pose() wpose.position.x = x_vec[i][0] wpose.position.y = x_vec[i][1] wpose.position.z = x_vec[i][2] wpose.orientation.x = x_vec[i][3] wpose.orientation.y = x_vec[i][4] wpose.orientation.z = x_vec[i][5] wpose.orientation.w = x_vec[i][6] waypoints.append(copy.deepcopy(wpose)) (plan, fraction) = group.compute_cartesian_path( waypoints, # waypoints to follow dt, # eef_step 0.0) # jump_threshold print "fraction: ", fraction print "plan: ", plan group.execute(plan)
def __init__(self): ## First initialize moveit_commander and rospy. moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('move_piece_interface', anonymous=True) ## Instantiate a RobotCommander object. This object is an interface to ## the robot as a whole. self.robot = moveit_commander.RobotCommander() ## Instantiate a PlanningSceneInterface object. This object is an interface ## to the world surrounding the robot. self.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. self.left_group = moveit_commander.MoveGroupCommander("left_arm") self.right_group = moveit_commander.MoveGroupCommander("right_arm") #self.listener = tf.TransformListener() #self.listener.waitForTransform("/base", 'left_gripper', rospy.Time(0), rospy.Duration(3.0)); #self._limb = baxter_interface.Limb('left') self._left_gripper = baxter_interface.Gripper('left') self._right_gripper = baxter_interface.Gripper('right') self.gripper_close()
def launch_script(): print "====== l_leg_inverse_ik.py started ========" print "======= Start initializing move commander for l_leg_inverse ===========" moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('l_leg_inverse_ik', anonymous=True) lleg_group = moveit_commander.MoveGroupCommander("l_leg_inverse") lleg_group.set_planner_id("RRTConnectkConfigDefault") print "\n =========== Some basic informations of Move Commander==================" print "Current Pose of end effector %s %s" %(lleg_group.get_end_effector_link(), lleg_group.get_current_pose()) lleg_group.set_end_effector_link("pelvis") print "The end effector is: %s" %lleg_group.get_end_effector_link() current_pose = lleg_group.get_current_pose() random_target = lleg_group.get_random_pose() print "===============================================================" print "Random pose set: %s" %random_target lleg_group.set_pose_target(random_target) print "===============================================================" print "Planning" lleg_group.plan() print "===============================================================" print "Running" lleg_group.go(wait=True) print "==============================================================="
def move_group_python_interface_tutorial(): ## First initialize moveit_commander and rospy. print "============ Starting tutorial setup" moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('move_group_python_interface_tutorial', anonymous=True) ## Instantiate a RobotCommander object. This object is an interface to ## the robot as a whole. robot = moveit_commander.RobotCommander() ## 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. group = moveit_commander.MoveGroupCommander("arm") ## Sometimes for debugging it is useful to print the entire state of the print "============ Printing robot end-effector pose" #print group.get_current_pose() print group.get_current_pose().pose.position.x, group.get_current_pose().pose.position.y, group.get_current_pose().pose.position.z print group.get_current_rpy() ## When finished shut down moveit_commander. moveit_commander.roscpp_shutdown()
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('subtask_grasp') rospy.on_shutdown(self.shutdown) self.grasp_srv = rospy.Service('subtask_grasp', rp_grasp, self.graspSrvCallback)
def __init__(self): rospy.init_node("moveit_cartesian_path", anonymous=False) rospy.loginfo("Starting node moveit_cartesian_path") rospy.on_shutdown(self.cleanup) moveit_commander.roscpp_initialize(sys.argv) self._arm = moveit_commander.MoveGroupCommander('manipulator') rospy.loginfo("End effector: " + self._arm.get_end_effector_link()) self._arm.set_pose_reference_frame("/base") # Allow replanning to increase the odds of a solution self._arm.allow_replanning(True) # Allow some leeway in position (meters) and orientation (radians) self._arm.set_goal_position_tolerance(0.01) self._arm.set_goal_orientation_tolerance(0.1) self._joint_order = ('shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint') self._joint_sub = rospy.Subscriber("/joint_states", JointState, self.cb_js) self._last_check = 0 self._tcp_location = None self._ur_kin = ur5()
def main(): global left_arm global left_gripper #Initialize moveit_commander moveit_commander.roscpp_initialize(sys.argv) #Start a node rospy.init_node('moveit_interface') #Set up Baxter Arms robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() left_arm = moveit_commander.MoveGroupCommander('left_arm') left_arm.set_planner_id('RRTConnectkConfigDefault') left_arm.set_planning_time(10) #Calibrate the end effector on the left arm left_gripper = baxter_gripper.Gripper('left') print('Calibrating...') left_gripper.calibrate() rospy.sleep(2.0) print('Ready to go') #make subscriber to position topic rospy.Subscriber('new_position', Pose, move_arm) #make subscriber to gripper topic rospy.Subscriber('gripper_control', Bool, actuate_gripper) rospy.spin()
def __init__(self): rospy.loginfo("Initializing ChipBehaviour") moveit_commander.roscpp_initialize(sys.argv) robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() self.group = moveit_commander.MoveGroupCommander("right_arm_torso") self.pose_pub = rospy.Publisher('/arm_pointing_pose', geometry_msgs.msg.PoseStamped, queue_size=1) self.tts_ac = SimpleActionClient('/tts', TtsAction) self.tts_ac.wait_for_server() self.point_head_ac = SimpleActionClient('/head_controller/point_head_action', PointHeadAction) self.point_head_ac.wait_for_server(rospy.Duration(10.0)) self.faces_sub = rospy.Subscriber('/pal_face/faces', FaceDetections, self.faces_cb, queue_size=1)
def listener(): # In ROS, nodes are uniquely named. If two nodes with the same # node are launched, the previous one is kicked off. The # anonymous=True flag means that rospy will choose a unique # name for our 'listener' node so that multiple listeners can # run simultaneously. moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('listener', anonymous=True) global robot global scene global arm global gripper robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() arm = moveit_commander.MoveGroupCommander("arm") gripper = moveit_commander.MoveGroupCommander("gripper") rospy.Subscriber("/pick_pose", Point, callback) # sub = rospy.Subscriber("/clicked_point", geometry_msgs.msg.PoseStamped, callback) # spin() simply keeps python from exiting until this node is stopped rospy.spin()
def InitializeMoveItCommander(): 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() rospy.sleep(1) ## Instantiate a PlanningSceneInterface object. This object is an interface ## to the world surrounding the robot. global scene scene = moveit_commander.PlanningSceneInterface() rospy.sleep(1) ## 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 group = MoveGroupCommander("left_arm") global left_gripper left_gripper = baxter_interface.Gripper('left') left_gripper.calibrate() rospy.sleep(2)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Initialize the MoveIt! commander for the right arm arm = moveit_commander.MoveGroupCommander('arm_with_torso') arm.set_end_effector_link('wrist_roll_link') # Get the name of the end-effector link end_effector_link = arm.get_end_effector_link() # Set the reference frame for pose targets reference_frame = 'base_link' # Set the right arm reference frame accordingly arm.set_pose_reference_frame(reference_frame) # 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.01) arm.set_goal_orientation_tolerance(0.05) # Set the start pose. This particular pose has the gripper oriented horizontally # 0.75 meters above the base and 0.72 meters in front of the robot. target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.header.stamp = rospy.Time.now() target_pose.pose.position.x = 0.609889 target_pose.pose.position.y = 0.207002 target_pose.pose.position.z = 1.10677 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 # Set the start state to the current state arm.set_start_state_to_current_state() # Set the goal pose of the end effector to the stored pose arm.set_pose_target(target_pose, end_effector_link) # Plan the trajectory to the goal traj = arm.plan() # Execute the planned trajectory arm.execute(traj) # Pause for a couple of seconds rospy.sleep(2) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit MoveIt moveit_commander.os._exit(0)
def __init__(self): smach.State.__init__(self, outcomes=['done'], input_keys=['goal_pose']) moveit_commander.roscpp_initialize(sys.argv) ## Instantiate a RobotCommander object. This object is an interface to ## the robot as a whole. self.robot = moveit_commander.RobotCommander() ## Instantiate a PlanningSceneInterface object. This object is an interface ## to the world surrounding the robot. rospy.loginfo('Instantiate PlanningSceneInterface') self.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. rospy.loginfo('Instantiate MoveGroupCommander') self.group = moveit_commander.MoveGroupCommander("manipulator") ## We create this DisplayTrajectory publisher which is used below to publish ## trajectories for RVIZ to visualize. rospy.loginfo('Create display_trajectory_publisher') self.display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory)
def move_arm(): print "Starting tutorial setup" moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('move_arm_node') print "move_arm_node should be initialized" print "Initializing Robot Commander" robot = moveit_commander.RobotCommander() print "Initializing Planning Scene Interface" scene = moveit_commander.PlanningSceneInterface() print "Let's move left arm first" group = moveit_commander.MoveGroupCommander("left_arm") print "Create publisher to display_planned_path" display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',moveit_msgs.msg.DisplayTrajectory) robot_pose = geometry_msgs.msg.Pose() robot_pose.orientation.x = 1 robot_pose.position.x = 0.7 robot_pose.position.y = 0.4 robot_pose.position.z = 0.3 group.set_pose_target(robot_pose) plan1 = group.plan() rospy.sleep(5) group.go(wait=True) moveit_commander.roscpp_shutdown() print "Finishing"
def move_group_python_interface_tutorial(): global group_left global group_right global p print "============ Starting tutorial setup" moveit_commander.roscpp_initialize(sys.argv) #rospy.init_node('move_group_python_interface_tutorial',anonymous=True) ## Instantiate a RobotCommander object. Interface to the robot. robot = moveit_commander.RobotCommander() ## Instantiate a PlanningSceneInterface object. Interface to the world. scene = moveit_commander.PlanningSceneInterface() ## Instantiate a MoveGroupCommander object. Interface to groups of joints. group_left = moveit_commander.MoveGroupCommander("left_arm") #group_right = moveit_commander.MoveGroupCommander("right_arm") ## 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, queue_size=20) ## Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up. print "============ Waiting for RVIZ..." #rospy.sleep(2) print "============ Starting tutorial " ## We can get the name of the reference frame for this robot print "============ Reference frame: %s" % group_left.get_planning_frame() ## We can also print the name of the end-effector link for this group print "============ End effector: %s" % group_left.get_end_effector_link() ## We can get a list of all the groups in the robot print "============ Robot Groups:" print robot.get_group_names() ## Sometimes for debugging it is useful to print the entire state of the ## robot. print "============ Printing robot state" print robot.get_current_state() print "============" p = PlanningSceneInterface("base_link") p.addBox("table",0.2 ,1.25 ,0.55 ,0.65 ,0.775 ,0.275) p.addCylinder("redCylinder",0.3,0.03,0.65,0.7,0.7) p.addCylinder("blueCylinder",0.3,0.03,0.65,0.8,0.7) p.addCylinder("greenCylinder",0.3,0.03,0.65,0.9,0.7) p.addBox("obstacle1_1", 0.6, 0.2, 0.4, 0.65, 0.45, 0.95) p.addBox("obstacle1_2", 0.45, 0.01, 0.50, 0.3, 0.4, 0.3) print group_left.get_planning_time() group_left.set_planning_time(15) print group_left.get_planning_time() group_left.set_num_planning_attempts(20) '''p.addBox("obstacle1_1", 0.15, 0.1, 0.3, 0.6, 0.4, 0.7)#llega hasta 0.85 p.addBox("obstacle1_2", 0.25, 0.1, 0.70, 0.35, 0.4, 0.35) #p.addBox("obstacle1_3", 0.75, 0.8, 0.3, 0.6, 0.4, 1.32) p.addBox("obstacle1_3", 0.75, 0.8, 0.1, 0.6, 0.4, 1.18) #p.addBox("obstacle1_3_1", 0.3, 0.1, 0.04, 0.225, 0.4, 1.09) p.addBox("obstacle1_1_bis", 0.3, 0.1, 0.4, 0.6, 0.00, 0.7)''' #p.addBox("obstacle1_2_bis", 0.25, 0.1, 0.1, 0.35, 0.4, 1.12) #p.addBox("obstacle1_11", 0.2, 0.1, 0.65, 0.6, 0.4, 1.175) #p.addBox("obstacle1_21", 0.25, 0.1, 0.65, 0.35, 0.4, 1.275) '''#PRUEBA 1
def __init__(self): # rospy.sleep(10) # wait for other files to be launched successfully moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('cw3_q1') self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) # self.touch = [0,0,0,0] # rospy.Subscriber("/contacts/lh_ff/distal", ContactsState, self.call_ff) # rospy.Subscriber("/contacts/lh_mf/distal", ContactsState, self.call_mf) # rospy.Subscriber("/contacts/lh_rf/distal", ContactsState, self.call_rf) # rospy.Subscriber("/contacts/lh_th/distal", ContactsState, self.call_th) # # Robot contains the entire state of the robot (iiwa and shadow hand) # robot = moveit_commander.RobotCommander() # # We can get a list of all the groups in the robot # print('============ Robot Groups:') # print('{}\n\n'.format(robot.get_group_names())) self.iiwa_group = moveit_commander.MoveGroupCommander('hand_iiwa') self.hand_group = moveit_commander.MoveGroupCommander('sr_hand') rospy.sleep(1) # object tracking [all_object_name,all_object_pose] = self.get_object_position() # create a service proxy for add and remove objects to allowed collision matrix add_to_acm = rospy.ServiceProxy('/add_object_acm', ChangeCollisionObject) remove_from_acm = rospy.ServiceProxy('/remove_object_acm', ChangeCollisionObject) # open the hand self.hand_open() for i in range(0,3): # i = 2 # object no. # move the iiwa to just above the object self.move_iiwa_to_object(all_object_pose,i) # add object to allowed collision matrix add_to_acm(all_object_name[i]) # close the hand (grasping) self.hand_close() # hold the grasping position for 10 seconds rospy.sleep(10) # open the hand (releasing) self.hand_open() # remove object from allowed collision matrix remove_from_acm(all_object_name[i]) moveit_commander.roscpp_shutdown() rospy.loginfo("Task finished")
def move_group_python_interface_tutorial(): global group_left global group_right global p print "============ Starting tutorial setup" moveit_commander.roscpp_initialize(sys.argv) #rospy.init_node('move_group_python_interface_tutorial',anonymous=True) ## Instantiate a RobotCommander object. Interface to the robot. robot = moveit_commander.RobotCommander() ## Instantiate a PlanningSceneInterface object. Interface to the world. scene = moveit_commander.PlanningSceneInterface() ## Instantiate a MoveGroupCommander object. Interface to groups of joints. group_left = moveit_commander.MoveGroupCommander("left_arm") #group_right = moveit_commander.MoveGroupCommander("right_arm") ## 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, queue_size=20) ## Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up. print "============ Waiting for RVIZ..." #rospy.sleep(2) print "============ Starting tutorial " ## We can get the name of the reference frame for this robot print "============ Reference frame: %s" % group_left.get_planning_frame() ## We can also print the name of the end-effector link for this group print "============ End effector: %s" % group_left.get_end_effector_link() ## We can get a list of all the groups in the robot print "============ Robot Groups:" print robot.get_group_names() ## Sometimes for debugging it is useful to print the entire state of the ## robot. print "============ Printing robot state" print robot.get_current_state() print "============" p = PlanningSceneInterface("base_link") p.addBox("table",0.2 ,1.25 ,0.35 ,0.6 ,0.775 ,0.175) p.addCylinder("redCylinder",0.3,0.03,0.6,0.7,0.5) p.addCylinder("blueCylinder",0.3,0.03,0.6,0.8,0.5) p.addCylinder("greenCylinder",0.3,0.03,0.6,0.9,0.5) p.addBox("obstacle1_1", 0.4, 0.01, 0.5, 0.6, 0.55, 0.6) p.addBox("obstacle1_2", 0.4, 0.01, 0.2, 0.6, 0.55, 0.95)#cambiar a 0.25 despues p.addBox("obstacle1_3", 0.4, 0.01, 0.2, 0.6, 0.55, 1.15) #p.addBox("obstacle1_1", 0.25, 0.01, 0.2, 0.6, 0.3, 0.45) #Estos estados para obstacle1 OK #p.addBox("obstacle1_2", 0.65, 0.01, 0.2, 0.6, 0.3, 0.65) #p.addBox("obstacle1_3", 0.65, 0.01, 0.2, 0.6, 0.3, 0.85) #p.addBox("obstacle2_1", 1, 0.01, 0.2, 0.6, 0.6, 0.45) #p.addBox("obstacle2_2", 0.25, 0.01, 0.2, 0.6, 0.6, 0.65) #.addBox("obstacle2_3", 0.95, 0.01, 0.2, 0.6, 0.6, 0.85) #p.addBox("obstacle2_4", 0.475, 0.01, 0.3, 0.2625, 0.4, 0.3) p.addBox("obstacle2_1", 0.15, 0.01, 0.45, 0.6, 0.3, 0.6) ## Planning to a Pose goal ## We can plan a motion for this group to a desired pose for the ## end-effector '''print "============ Running code from ~/ws_moveit"
def kinect_controller(): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('robot_state_publisher', anonymous=True,) rate = rospy.Rate(10) # Instantiate a RobotCommander object. robotc = moveit_commander.RobotCommander() groupc = moveit_commander.MoveGroupCommander("Kinect2_Target") groupc.set_planner_id("RRTConnectkConfigDefault") # We create this publisher for the desired pose of the Kinect2 Sensor pose_publisher = rospy.Publisher("desired_pose", PoseStamped, queue_size=5) # Create a suscrber to read the trajectory published by kinect_planner.py rospy.Subscriber("planned_path", RobotTrajectory, callback) print "=============== Current Pose ===========" print groupc.get_current_pose(); print "========= Setting initial pose ================" group_variable_values = groupc.get_current_joint_values() ## Defining a Pose goal pose_target = PoseStamped() pose_target.header.frame_id = 'base_footprint' pose_target.pose.orientation.x = 0 pose_target.pose.orientation.y = 0 pose_target.pose.orientation.z = 0 pose_target.pose.orientation.w = 1 pose_target.pose.position.x = 1.3 pose_target.pose.position.y = -0.2 pose_target.pose.position.z = 1.17 while not rospy.is_shutdown(): global fresh_data # Publishe desired pose pose_publisher.publish(pose_target) fresh_data = false # If no trajectory is received, sleep while fresh_data == false: rate.sleep() # Read the trajectory published by kinect_planner.py #plan = set_robot_trajectory_msg(trajectory1,group_variable_values) plan_target = groupc.plan(trajectory1) groupc.go(wait=True) print "============ Waiting while the plan is executed" rate.sleep() # Display current joint values group_variable_values = groupc.get_current_joint_values() print "============ Joint values: ", group_variable_values
def init(): #Wake up Baxter baxter_interface.RobotEnable().enable() rospy.sleep(0.25) print "Baxter is enabled" print "Intitializing clients for services" global ik_service_left ik_service_left = rospy.ServiceProxy( "ExternalTools/left/PositionKinematicsNode/IKService", SolvePositionIK) global ik_service_right ik_service_right = rospy.ServiceProxy( "ExternalTools/right/PositionKinematicsNode/IKService", SolvePositionIK) global obj_loc_service obj_loc_service = rospy.ServiceProxy( "object_location_service", ObjLocation) global stopflag stopflag = False #Taken from the MoveIt Tutorials moveit_commander.roscpp_initialize(sys.argv) robot = moveit_commander.RobotCommander() global scene scene = moveit_commander.PlanningSceneInterface() #Activate Left Arm to be used with MoveIt global left_group left_group = MoveGroupCommander("left_arm") left_group.set_goal_position_tolerance(0.01) left_group.set_goal_orientation_tolerance(0.01) global right_group right_group = MoveGroupCommander("right_arm") pose_right = Pose() pose_right.position = Point(0.793, -0.586, 0.329) pose_right.orientation = Quaternion(1.0, 0.0, 0.0, 0.0) request_pose(pose_right, "right", right_group) global left_gripper left_gripper = baxter_interface.Gripper('left') left_gripper.calibrate() print left_gripper.parameters() global end_effector_subs end_effector_subs = rospy.Subscriber("/robot/end_effector/left_gripper/state", EndEffectorState, end_effector_callback) rospy.sleep(1) global pubpic pubpic = rospy.Publisher('/robot/xdisplay', Image, latch=True, queue_size=1) rospy.sleep(1)
def publish_states(): moveit_commander.roscpp_initialize(sys.argv) joint_states = JointState() rospy.init_node('Testing_node') joint_states.name = ['r1joint_1', 'r1joint_2', 'r1joint_3', 'r1joint_4', 'r1joint_5', 'r1joint_6'] joint_states.position = [1.2,1,1,-0.2,-0.2,-0.2] joint_states.header.frame_id = '/base_link' set_joints(joint_states)
def main(): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit', anonymous=True) rospy.Subscriber("point", Point, callback) rospy.loginfo("Starting moveit") try: rospy.spin() except KeyboardInterrupt: print("Shutting down")
def go(): node_name = 'mico_planner' group_name = 'arm' planner_name = 'RRTstarkConfigDefault' ee_link_name = 'mico_link_endeffector' roscpp_initialize(sys.argv) rospy.init_node(node_name, anonymous=True) acHan = ActionHandler(group_name, planner_name, ee_link_name) rospy.sleep(1) files = [] lant = -1 loc = -1 ori = -1 #get input has_input = False while(not has_input): inp = raw_input("Enter Lantern to pick, and location to place (Format = Lantern# Location# Orientation#) > ") inp = inp.split(" ") if len(inp) == 3: for n in inp: if (n == "0") or (n == "1") or (n == "2") or (n == "3") or (n == "4"): has_input = True else: has_input = False break lant = inp[0] loc = inp[1] ori = inp[2] #get files files.append("StartupPlan") files.append("Release") files.append("TransEmpty_" + str(lant) + "_" + str(ori)) files.append("Grasp_80") files.append("TransLoadedUp_" + str(lant) + "_" + str(ori)) files.append("TransLoadedDown_" + str(loc) + "_" + str(ori)) files.append("Grasp_30") files.append("TransEmptyRetract_" + str(loc) + "_" + str(ori)) files.append("Release") for f in files: t = open("plans/" + str(f), "r") runFiles(t, acHan) t.close()
def initialize_moveit(self): print "============ Initializing Moveit" moveit_commander.roscpp_initialize(sys.argv) #rospy.init_node('baxter3_moveit',anonymous=True) ## Instantiate a RobotCommander object. This object is an interface to ## the robot as a whole. self.robot = moveit_commander.RobotCommander() ## Instantiate a PlanningSceneInterface object. This object is an interface ## to the world surrounding the robot. self.scene = moveit_commander.PlanningSceneInterface() print "============ Waiting for RVIZ..." rospy.sleep(3) print "============ Starting tutorial " print "============ Adding collision objects..." # Add in objects # Table p = PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.pose.position.x = 0.8 p.pose.position.y = 0.025 p.pose.position.z = -0.6 p.pose.orientation.x = 1 self.scene.add_box("table", p, (0.762, 1.25, 1)) # Box 1 p = PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.pose.position.x = 0.6 p.pose.position.y = 0.025 p.pose.position.z = -0.22 p.pose.orientation.x = 1 self.scene.add_box("box1", p, (0.25, 0.2, 0.08)) # Box 2 p = PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.pose.position.x = 1 p.pose.position.y = 0.025 p.pose.position.z = -0.22 p.pose.orientation.x = 1 self.scene.add_box("box2", p, (0.25, 0.2, 0.08)) print "============" self.group_left = moveit_commander.MoveGroupCommander("left_arm") self.group_left.set_goal_position_tolerance(0.01) self.group_left.set_goal_orientation_tolerance(0.01) self.group_right = moveit_commander.MoveGroupCommander("right_arm") self.group_right.set_goal_position_tolerance(0.01) self.group_right.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, queue_size=10)
def __init__(self): rospy.init_node('cruimanager') moveit_commander.roscpp_initialize(sys.argv) # Pull all params off param server self.analyze_grasp_topic = rospy.get_param("analyze_grasp_topic") self.execute_grasp_topic = rospy.get_param("execute_grasp_topic") self.run_recognition_topic = rospy.get_param("run_recognition_topic") self.grasp_approach_tran_frame = rospy.get_param("grasp_approach_tran_frame") self.world_frame = rospy.get_param("world_frame") self.arm_move_group_name = rospy.get_param("arm_move_group_name") self.gripper_move_group_name = rospy.get_param("gripper_move_group_name") self.analyzer_planner_id = rospy.get_param("analyzer_planner_id") self.executor_planner_id = rospy.get_param("executor_planner_id") self.allowed_analyzing_time = rospy.get_param("allowed_analyzing_time") self.allowed_execution_time = rospy.get_param("allowed_execution_time") self.grasping_controller = graspit_moveit_controller.MoveitPickPlaceInterface( arm_name=self.arm_move_group_name, gripper_name=self.gripper_move_group_name, grasp_approach_tran_frame=self.grasp_approach_tran_frame, analyzer_planner_id=self.analyzer_planner_id, execution_planner_id=self.executor_planner_id, allowed_analyzing_time=self.allowed_analyzing_time, allowed_execution_time=self.allowed_execution_time ) self.scene = moveit_commander.PlanningSceneInterface() self.block_recognition_client = block_recognition.BlockRecognitionClient() self.world_manager_client = world_manager.world_manager_client.WorldManagerClient() self.tf_listener = tf.TransformListener() # Start grasp analyzer action server self._analyze_grasp_as = actionlib.SimpleActionServer(self.analyze_grasp_topic, graspit_msgs.msg.CheckGraspReachabilityAction, execute_cb=self._analyze_grasp_reachability_cb, auto_start=False) self._analyze_grasp_as.start() # Start grasp execution action server self._execute_grasp_as = actionlib.SimpleActionServer(self.execute_grasp_topic, graspit_msgs.msg.GraspExecutionAction, execute_cb=self._execute_grasp_cb, auto_start=False) self._execute_grasp_as.start() # Start object recognition action server self._run_recognition_as = actionlib.SimpleActionServer(self.run_recognition_topic, graspit_msgs.msg.RunObjectRecognitionAction, execute_cb=self._run_recognition_cb, auto_start=False) self._run_recognition_as.start() rospy.loginfo(self.__class__.__name__ + " is inited")
def __init__(self): moveit_commander.roscpp_initialize(sys.argv) # initialize MoveIt self.robot = moveit_commander.RobotCommander() self.scene = moveit_commander.PlanningSceneInterface() self.leftGroup = moveit_commander.MoveGroupCommander("left_arm") self.rightGroup = moveit_commander.MoveGroupCommander("right_arm") self.display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory)
def __init__(self): rospy.init_node('xm_arm_moveit_ik_test', anonymous=True) moveit_commander.roscpp_initialize(sys.argv) xm_arm = moveit_commander.MoveGroupCommander('xm_arm') end_effector_link = xm_arm.get_end_effector_link() reference_frame = 'base_footprint' xm_arm.set_pose_reference_frame(reference_frame) xm_arm.allow_replanning(True) xm_arm.set_goal_position_tolerance(5) xm_arm.set_goal_orientation_tolerance(5) xm_arm.set_planner_id("RRTConnectkConfigDefault") xm_arm.set_named_target('initial') xm_arm.go() rospy.sleep(2) print "-------------------- Test Arm Moveit IK ----------------------" print "Input the position of object(reference frame is base's center)" print "Such as [object/o x y z x y z w] " while not rospy.is_shutdown(): print ">>> ", keyboard_cmd = raw_input().split(" ") try: if keyboard_cmd[0] == "object" or keyboard_cmd[0] == "o": target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.header.stamp = rospy.Time.now() target_pose.pose.position.x = float(keyboard_cmd[1]) target_pose.pose.position.y = float(keyboard_cmd[2]) target_pose.pose.position.z = float(keyboard_cmd[3]) target_pose.pose.orientation.x = float(keyboard_cmd[4]) target_pose.pose.orientation.y = float(keyboard_cmd[5]) target_pose.pose.orientation.z = float(keyboard_cmd[6]) target_pose.pose.orientation.w = float(keyboard_cmd[7]) print ("position[x y z] %s, %s, %s" % (keyboard_cmd[1], keyboard_cmd[2], keyboard_cmd[3])) print "orientation[x y z w] %s, %s, %s, %s" % ( keyboard_cmd[4], keyboard_cmd[5], keyboard_cmd[6], keyboard_cmd[7]) xm_arm.set_start_state_to_current_state() xm_arm.set_pose_target(target_pose, end_effector_link) trajectory = xm_arm.plan() xm_arm.execute(trajectory) rospy.sleep(1) elif keyboard_cmd[0] == "exit" or keyboard_cmd[0] == "e": exit() except Exception as exce: print "Error!", exce moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): # Initialize API for movo_group moveit_commander.roscpp_initialize(sys.argv) # Initialize Node of ROS rospy.init_node('zhiyang_attached_camera_demo') # Define Scene Object scene = PlanningSceneInterface() rospy.sleep(1) # Initialize arm group from moveit group to control manipulator rarm = MoveGroupCommander('right_arm') larm = MoveGroupCommander('left_arm') # Get end effector link name r_end_effector_link = rarm.get_end_effector_link() l_end_effector_link = larm.get_end_effector_link() # Set tolerance of position(m) and orentation(rad) rarm.set_goal_position_tolerance(0.01) rarm.set_goal_orientation_tolerance(0.05) larm.set_goal_position_tolerance(0.01) larm.set_goal_orientation_tolerance(0.05) # Allow replanning rarm.allow_replanning(True) rarm.set_planning_time(10) larm.allow_replanning(True) larm.set_planning_time(10) # Let arm go back to home position # arm.set_named_target('home') # arm.go() # Remove other attached object in before Scene run # scene.remove_attached_object(end_effector_link, 'tool') # scene.remove_world_object('table') # scene.remove_world_object('target') # Set height of table table_ground = 0.6 # Set 3d size of table and tool table_size = [0.3, 0.7, 0.01] tool_size = [0.15, 0.15, 0.06] # Set orentation and position of tool p1 = PoseStamped() p1.header.frame_id = r_end_effector_link # p.pose.position.x = tool_size[0] / 2.0 - 0.025 # p.pose.position.y = -0.015 # p.pose.position.z = 0.0 p1.pose.position.x = -0.07 p1.pose.position.y = 0.0 p1.pose.position.z = 0.08 p1.pose.orientation.x = 0 p1.pose.orientation.y = 0 p1.pose.orientation.z = 0 p1.pose.orientation.w = 1 p2 = PoseStamped() p2.header.frame_id = l_end_effector_link p2.pose.position.x = -0.07 p2.pose.position.y = 0.0 p2.pose.position.z = 0.08 p2.pose.orientation.x = 0 p2.pose.orientation.y = 0 p2.pose.orientation.z = 0 p2.pose.orientation.w = 1 # Make tool attact to end effector scene.attach_box(r_end_effector_link, 'Camera1', p1, tool_size) scene.attach_box(l_end_effector_link, 'Camera2', p2, tool_size) # Add table to Scene table_pose = PoseStamped() table_pose.header.frame_id = 'base_link' table_pose.pose.position.x = 0.75 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) rospy.sleep(2) # Update current state (position and orentation) rarm.set_start_state_to_current_state() larm.set_start_state_to_current_state() # Set target (rad) joint_positions = [0.827228546495185, 0.29496592875743577, 0.29496592875743577, 1.1185644936946095, -0.7987583317769674, -0.18950024740190782, 0.11752152218233858] rarm.set_joint_value_target(joint_positions) larm.set_joint_value_target(joint_positions) # Let arm go rarm.go() larm.go() rospy.sleep(1) # Let arm go back to initial state # arm.set_named_target('home') # arm.go() moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def main(): #Main Function that initializes the node moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('elir_move_group', anonymous=True) x = MoveGroupElir()
def __init__(self): moveit_commander.roscpp_initialize(sys.argv) current_robot_ns = 'robot1' self.robot = moveit_commander.RobotCommander(current_robot_ns+'/robot_description',current_robot_ns) # Init moveit group self.group = moveit_commander.MoveGroupCommander('robot', current_robot_ns+'/robot_description',current_robot_ns) self.arm_group = moveit_commander.MoveGroupCommander('arm',current_robot_ns+'/robot_description',current_robot_ns) self.base_group = moveit_commander.MoveGroupCommander('base', current_robot_ns+'/robot_description',current_robot_ns) self.scene = moveit_commander.PlanningSceneInterface() self.sce = moveit_python.PlanningSceneInterface('odom', current_robot_ns) self.pub_co = rospy.Publisher('/'+current_robot_ns+'/collision_object', CollisionObject, queue_size=100) self.msg_print = SetBoolRequest() self.request_fk = rospy.ServiceProxy('/'+current_robot_ns+'/compute_fk', GetPositionFK) self.pub_co = rospy.Publisher('/'+current_robot_ns+'/collision_object',CollisionObject,queue_size=10) sub_waypoint_status = rospy.Subscriber('/'+current_robot_ns+'/execute_trajectory/status', GoalStatusArray, self.waypoint_execution_cb) sub_movegroup_status = rospy.Subscriber('/'+current_robot_ns+'/move_group/status', GoalStatusArray, self.move_group_execution_cb) rospy.Subscriber('/'+current_robot_ns+"/joint_states", JointState, self.further_ob_printing) # Initialize extruder print_request = rospy.ServiceProxy('/'+current_robot_ns+'/set_extruder_printing', SetBool) self.extruder_publisher = rospy.Publisher('/'+current_robot_ns+'/set_extruder_rate',Float32,queue_size=20) # switch the extruder ON, control via set_extruder_rate msg_print = SetBoolRequest() msg_print.data=True print_request(msg_print) self.re_position_x = [] self.re_position_y = [] self.waypoint_execution_status = 0 self.move_group_execution_status = 0 self.further_printing_number = 0 self.pre_further_printing_number = 0 # initial printing number self._printing_number = 0 self._further_printing_number = 0 self.future_printing_status = False self.current_printing_pose = None self.previous_printing_pose = None self.target_list = None self.group.allow_looking(1) self.group.allow_replanning(1) self.group.set_planning_time(10) # Initialize time record self.travel_time = 0 self.planning_time = 0 self.printing_time = 0 # Initialize extruder msg_extrude = Float32() msg_extrude = 0 self.extruder_publisher.publish(msg_extrude)
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(True) # 设置目标位置所使用的参考坐标系 arm.set_pose_reference_frame('base_link') # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.1) # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 控制机械臂运动到之前设置的“forward”姿态 arm.set_named_target('forward') arm.go() # 获取当前位姿数据最为机械臂运动的起始位姿 start_pose = arm.get_current_pose(end_effector_link).pose # 初始化路点列表 waypoints = [] # 将初始位姿加入路点列表 if cartesian: waypoints.append(start_pose) # 设置第二个路点数据,并加入路点列表 # 第二个路点需要向后运动0.2米,向右运动0.2米 wpose = deepcopy(start_pose) #wpose.position.x -= 0.05 wpose.position.y += 0.1 #wpose.position.z += 0.2 if cartesian: waypoints.append(deepcopy(wpose)) else: arm.set_pose_target(wpose) arm.go() rospy.sleep(1) # 设置第三个路点数据,并加入路点列表 #wpose.position.x -= 0.05 wpose.position.y += 0.15 #wpose.position.z += 0.15 if cartesian: waypoints.append(deepcopy(wpose)) else: arm.set_pose_target(wpose) arm.go() rospy.sleep(1) # 设置第四个路点数据,回到初始位置,并加入路点列表 if cartesian: waypoints.append(deepcopy(start_pose)) else: arm.set_pose_target(start_pose) arm.go() rospy.sleep(1) if cartesian: fraction = 0.0 #路径规划覆盖率 maxtries = 100 #最大尝试规划次数 attempts = 0 #已经尝试规划次数 # 设置机器臂当前的状态作为运动初始状态 arm.set_start_state_to_current_state() # 尝试规划一条笛卡尔空间下的路径,依次通过所有路点 while fraction < 1.0 and attempts < maxtries: (plan, fraction) = arm.compute_cartesian_path( waypoints, # waypoint poses,路点列表 0.01, # eef_step,终端步进值 0.0, # jump_threshold,跳跃阈值 True) # avoid_collisions,避障规划 # 尝试次数累加 attempts += 1 # 打印运动规划进程 if attempts % 10 == 0: rospy.loginfo("Still trying after " + str(attempts) + " attempts...") # 如果路径规划成功(覆盖率100%),则开始控制机械臂运动 if fraction == 1.0: rospy.loginfo("Path computed successfully. Moving the arm.") arm.execute(plan) rospy.loginfo("Path execution complete.") # 如果路径规划失败,则打印失败信息 else: rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.") # 控制机械臂回到初始化位置 arm.set_named_target('home') arm.go() rospy.sleep(1) # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('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('aubo10') # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.05) # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) # 设置每次运动规划的时间限制:5s arm.set_planning_time(5) # 设置场景物体的名称 table_id = 'table' box1_id = 'box1' box2_id = 'box2' # 移除场景中之前运行残留的物体 scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) rospy.sleep(1) # 控制机械臂先回到初始化位置 # target_pose = PoseStamped() # target_pose.header.frame_id = reference_frame # target_pose.pose.position.x = 0.0 # target_pose.pose.position.y = 0.0 # target_pose.pose.position.z = 0.0 # target_pose.pose.orientation.w = 1.0 # # 控制机械臂运动到目标位置 # arm.set_pose_target(target_pose, end_effector_link) # arm.go() # rospy.sleep(2) # 设置桌面的高度 table_ground = 1.4 # 设置table、box1和box2的三维尺寸 table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # 将三个物体加入场景当中 table_pose = PoseStamped() table_pose.header.frame_id = reference_frame table_pose.pose.position.x = 0.8 table_pose.pose.position.y = -0.9 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.8 # box1_pose.pose.position.y = -1.5 # 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.8 # box2_pose.pose.position.y = -0.1 # box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0 # box2_pose.pose.orientation.w = 1.0 # scene.add_box(box2_id, box2_pose, box2_size) # 将桌子设置成红色,两个box设置成橙色 self.setColor(table_id, 0.8, 0, 0, 1.0) # self.setColor(box1_id, 0.8, 0.4, 0, 1.0) # self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # 将场景中的颜色设置发布 self.sendColors() # 设置机械臂的运动目标位置,位于桌面之上两个盒子之间 target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.pose.position.x = 0.8 target_pose.pose.position.y = -0.7 target_pose.pose.position.z = 1.5 #table_pose.pose.position.z + table_size[2] + 0.05 target_pose.pose.orientation.w = 1.0 # 控制机械臂运动到目标位置 arm.set_pose_target(target_pose, end_effector_link) arm.go() rospy.sleep(2) # # 设置机械臂的运动目标位置,进行避障规划 # target_pose2 = PoseStamped() # target_pose2.header.frame_id = reference_frame # target_pose2.pose.position.x = 0.8 # target_pose2.pose.position.y = -0.7 # target_pose2.pose.position.z = 1.5#table_pose.pose.position.z + table_size[2] + 0.05 # target_pose2.pose.orientation.w = 1.0 # # 控制机械臂运动到目标位置 # arm.set_pose_target(target_pose2, end_effector_link) # arm.go() # rospy.sleep(2) # target_pose = PoseStamped() # target_pose.header.frame_id = reference_frame # target_pose.pose.position.x = 0.0 # target_pose.pose.position.y = 0.0 # target_pose.pose.position.z = 0.0 # target_pose.pose.orientation.w = 1.0 # # 控制机械臂运动到目标位置 # arm.set_pose_target(target_pose, end_effector_link) # arm.go() # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): ''' Constructor of the MoveitObjectHandler class. ''' moveit_commander.roscpp_initialize(sys.argv) self.scene = moveit_commander.PlanningSceneInterface()
def __init__(self): self.bridge = cv_bridge.CvBridge() self.image_sub = rospy.Subscriber('/ur5/usbcam/image_raw', Image, self.image_callback) rospy.init_node("moveit_cartesian_path", anonymous=False) rospy.loginfo("Starting node moveit_cartesian_path") rospy.on_shutdown(self.cleanup) # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) # Initialize the move group for the ur5_arm self.arm = moveit_commander.MoveGroupCommander('manipulator') # Get the name of the end-effector link end_effector_link = self.arm.get_end_effector_link() # Set the reference frame for pose targets reference_frame = "/base_link" # Set the ur5_arm reference frame accordingly self.arm.set_pose_reference_frame(reference_frame) # Allow replanning to increase the odds of a solution self.arm.allow_replanning(True) # Allow some leeway in position (meters) and orientation (radians) self.arm.set_goal_position_tolerance(0.01) self.arm.set_goal_orientation_tolerance(0.1) # Get the current pose so we can add it as a waypoint start_pose = self.arm.get_current_pose(end_effector_link).pose # Initialize the waypoints list waypoints = [] # Set the first waypoint to be the starting pose # Append the pose to the waypoints list wpose = deepcopy(start_pose) # Set the next waypoint to the right 0.5 meters wpose.position.x = -0.2 wpose.position.y = -0.2 wpose.position.z = 0.3 waypoints.append(deepcopy(wpose)) wpose.position.x = 0.1052 wpose.position.y = -0.4271 wpose.position.z = 0.4005 wpose.orientation.x = 0.4811 wpose.orientation.y = 0.4994 wpose.orientation.z = -0.5121 wpose.orientation.w = 0.5069 waypoints.append(deepcopy(wpose)) if np.sqrt((wpose.position.x-start_pose.position.x)**2+(wpose.position.x-start_pose.position.x)**2 \ +(wpose.position.x-start_pose.position.x)**2)<0.1: rospy.loginfo( "Warnig: target position overlaps with the initial position!") # self.arm.set_pose_target(wpose) # Set the internal state to the current state self.arm.set_start_state_to_current_state() # Plan the Cartesian path connecting the waypoints """moveit_commander.move_group.MoveGroupCommander.compute_cartesian_path( self, waypoints, eef_step, jump_threshold, avoid_collisios= True) Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the poses specified as waypoints. Configurations are computed for every eef_step meters; The jump_threshold specifies the maximum distance in configuration space between consecutive points in the resultingpath. The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory. """ plan, fraction = self.arm.compute_cartesian_path( waypoints, 0.01, 0.0, True) # plan = self.arm.plan() # If we have a complete plan, execute the trajectory if 1 - fraction < 0.2: rospy.loginfo("Path computed successfully. Moving the arm.") num_pts = len(plan.joint_trajectory.points) rospy.loginfo("\n# intermediate waypoints = " + str(num_pts)) self.arm.execute(plan) rospy.loginfo("Path execution complete.") else: rospy.loginfo("Path planning failed")
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) GRIPPER_OPEN = [0.05] GRIPPER_CLOSED = [-0.03] GRIPPER_NEUTRAL = [0.01] # Connect to the right_arm move group right_arm = moveit_commander.MoveGroupCommander('right_arm') # Connect to the right_gripper move group right_gripper = moveit_commander.MoveGroupCommander('right_gripper') # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Display the name of the end_effector link rospy.loginfo("The end effector link is: " + str(end_effector_link)) # Set a small tolerance on joint angles right_arm.set_goal_joint_tolerance(0.001) right_gripper.set_goal_joint_tolerance(0.001) # Start the arm target in "resting" pose stored in the SRDF file right_arm.set_named_target('resting') # Plan a trajectory to the goal configuration traj = right_arm.plan() # Execute the planned trajectory right_arm.execute(traj) # Pause for a moment rospy.sleep(1) # Set the gripper target to neutal position using a joint value target right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) # Plan and execute the gripper motion right_gripper.go() rospy.sleep(1) # Set target joint values for the arm: joints are in the order they appear in # the kinematic tree. joint_positions = [-0.0867, -1.274, 0.02832, 0.0820, -1.273, -0.003] # Set the arm's goal configuration to the be the joint positions right_arm.set_joint_value_target(joint_positions) # Plan and execute the motion right_arm.go() rospy.sleep(1) # Save this configuration for later right_arm.remember_joint_values('saved_config', joint_positions) # Close the gripper as if picking something up right_gripper.set_joint_value_target(GRIPPER_CLOSED) right_gripper.go() rospy.sleep(1) # Set the arm target to the named "straight_out" pose stored in the SRDF file right_arm.set_named_target('straight_forward') # Plan and execute the motion right_arm.go() rospy.sleep(1) # Set the goal configuration to the named configuration saved earlier right_arm.set_named_target('saved_config') # Plan and execute the motion right_arm.go() rospy.sleep(1) # Open the gripper as if letting something go right_gripper.set_joint_value_target(GRIPPER_OPEN) right_gripper.go() rospy.sleep(1) # Return the arm to the named "resting" pose stored in the SRDF file right_arm.set_named_target('resting') right_arm.go() rospy.sleep(1) # Return the gripper target to neutral position right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Cleanly shut down MoveIt moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Construct the initial scene object scene = PlanningSceneInterface() 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 both_arms = moveit_commander.MoveGroupCommander('both_arms') right_arm = moveit_commander.MoveGroupCommander('right_arm') left_arm = moveit_commander.MoveGroupCommander('left_arm') left_gripper = MoveGroupCommander('left_gripper') # Get the name of the end-effector link right_eef = right_arm.get_end_effector_link() left_eef = left_arm.get_end_effector_link() print "============ Printing end-effector link" print right_eef print left_eef # Set the reference frame for pose targets right_reference_frame = right_arm.get_planning_frame() left_reference_frame = left_arm.get_planning_frame() # Set the right arm reference frame accordingly #right_arm.set_pose_reference_frame(reference_frame) # 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.01) right_arm.set_goal_orientation_tolerance(0.01) left_arm.allow_replanning(True) # Allow some leeway in position (meters) and orientation (radians) left_arm.set_goal_position_tolerance(0.01) left_arm.set_goal_orientation_tolerance(0.01) # Allow 5 seconds per planning attempt left_arm.set_planning_time(5) right_arm.set_planning_time(5) object1_id = 'object1' # Remove leftover objects from a previous run scene.remove_world_object(object1_id) rospy.sleep(2) right_arm.set_named_target('right_ready') right_arm.go() left_arm.set_named_target('left_ready') left_arm.go() left_gripper.set_joint_value_target(GRIPPER_OPEN) left_gripper.go() rospy.sleep(1) # attach_box # Set the length, width and height of the object to attach object1_size = [0.09, 0.057, 0.001] # Create a pose for the tool relative to the end-effector object1_p = PoseStamped() object1_p.header.frame_id = right_reference_frame object1_p.pose.position.x = 0.77 object1_p.pose.position.y = -0.27 object1_p.pose.position.z = -0.21 object1_p.pose.orientation.w = 1 scene.add_box(object1_id, object1_p, object1_size) self.setColor(object1_id, 0.8, 0.4, 0, 1.0) self.sendColors() right_arm.set_start_state_to_current_state() left_arm.set_start_state_to_current_state() # target pose target_pose1 = PoseStamped() target_pose1.header.frame_id = right_reference_frame target_pose1.header.stamp = rospy.Time.now() target_pose1.pose.position.x = 0.77 target_pose1.pose.position.y = -0.27 target_pose1.pose.position.z = -0.14 target_pose1.pose.orientation.x = 0.500166303975 target_pose1.pose.orientation.y = 0.865326245156 target_pose1.pose.orientation.z = -0.0155702691449 target_pose1.pose.orientation.w = 0.0283147405248 right_arm.set_pose_target(target_pose1, right_eef) right_arm.go() rospy.sleep(1) # Attach the tool to the end-effector scene.attach_box(right_eef, object1_id, object1_p, object1_size) target_pose2 = PoseStamped() target_pose2.header.frame_id = right_reference_frame target_pose2.header.stamp = rospy.Time.now() target_pose2.pose.position.x = 0.632 target_pose2.pose.position.y = -0.125 target_pose2.pose.position.z = 0.08 target_pose2.pose.orientation.x = 0.2392 target_pose2.pose.orientation.y = -0.9708 target_pose2.pose.orientation.z = -0.0137 target_pose2.pose.orientation.w = 0.0103 target_pose3 = PoseStamped() target_pose3.header.frame_id = left_reference_frame target_pose3.header.stamp = rospy.Time.now() target_pose3.pose.position.x = 0.632 target_pose3.pose.position.y = 0.026 target_pose3.pose.position.z = 0.013 target_pose3.pose.orientation.x = -0.021 target_pose3.pose.orientation.y = 0.017 target_pose3.pose.orientation.z = -0.999 target_pose3.pose.orientation.w = 0.031 both_arms.set_pose_target(target_pose2, right_eef) both_arms.set_pose_target(target_pose3, left_eef) both_arms.go() rospy.sleep(3) #scene.remove_attached_object(right_eef, 'tool') object1_p = left_arm.get_current_pose(left_eef) object1_p.pose.position.y -= 0.12 object1_p.pose.orientation.x = 0 object1_p.pose.orientation.y = 0 object1_p.pose.orientation.z = 0.707 object1_p.pose.orientation.w = 0.707 scene.remove_attached_object(right_eef, object1_id) scene.attach_box(left_eef, object1_id, object1_p, object1_size) rospy.sleep(1) left_gripper.set_joint_value_target(GRIPPER_CLOSE) left_gripper.go() target_pose4 = PoseStamped() target_pose4.header.frame_id = right_reference_frame target_pose4.header.stamp = rospy.Time.now() target_pose4.pose.position.x = 0.58241 target_pose4.pose.position.y = -0.30286 target_pose4.pose.position.z = 0.05471 target_pose4.pose.orientation.x = 0.23931 target_pose4.pose.orientation.y = -0.97080 target_pose4.pose.orientation.z = -0.01346 target_pose4.pose.orientation.w = 0.01003 target_pose5 = PoseStamped() target_pose5.header.frame_id = left_reference_frame target_pose5.header.stamp = rospy.Time.now() target_pose5.pose.position.x = 0.843 target_pose5.pose.position.y = 0.438 target_pose5.pose.position.z = 0.105 target_pose5.pose.orientation.x = -0.01386 target_pose5.pose.orientation.y = -0.02950 target_pose5.pose.orientation.z = -0.70489 target_pose5.pose.orientation.w = 0.70856 #right_arm.set_pose_target(target_pose4, right_eef) both_arms.set_pose_target(target_pose4, right_eef) both_arms.set_pose_target(target_pose5, left_eef) both_arms.go() rospy.sleep(2) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit MoveIt moveit_commander.os._exit(0)
joint_goal[1] = joint_goal_list[1] joint_goal[2] = joint_goal_list[2] joint_goal[3] = joint_goal_list[3] joint_goal[4] = joint_goal_list[4] joint_goal[5] = joint_goal_list[5] #제어시작 move_group.go(joint_goal, wait=False) if __name__=='__main__': signal.signal(signal.SIGINT, signal_handler) #GROUP_NAME_GRIPPER = "NAME OF GRIPPER" roscpp_initialize(sys.argv) rospy.init_node('control_Husky_UR3', anonymous=True) robot = RobotCommander() scene = PlanningSceneInterface() ##모바일 파트 관련 변수 선언 x = 0.0 y = 0.0 theta = 0.0 ## 매니퓰레이터 변수 선언
def move_demo(): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_personal_demo', anonymous=True) robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() group = moveit_commander.MoveGroupCommander("left_arm") display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) rospy.sleep(10) #allow RVIZ to initialize?? print("============ Reference frame: %s" % group.get_planning_frame()) print("=========== Generating plan 1") pose_target = geometry_msgs.msg.Pose() pose_target.orientation.w = 1.0 pose_target.position.x = 0.7 pose_target.position.y = -0.05 pose_target.position.z = 1.1 group.set_pose_target(pose_target) plan1 = group.plan() print("========= Waiting while RVIZ displays play1...") rospy.sleep(10) collision_object = moveit_msgs.msg.CollisionObject() collision_object.header.frame_id = group.get_planning_frame() collision_object.id = 'box1' # primitive = shape_msgs.msg.SolidPrimitive() # primitive.type = primitive.BOX # primitive.dimensions.append(0.4) # primitive.dimensions.append(0.1) # primitive.dimensions.append(0.4) # collision_object.primitives.append(primitive) # collision_object.primitive_poses.append(box_pose) # collision_object.operation = collision_object.ADD # collision_objects = list(collision_objects) # print("================= Trying to add box1") # box1_pose = geometry_msgs.msg.Pose() # box1_pose.orientation.w = 1.0 # box1_pose.position.x = 0.5 # box1_pose.position.y = 0.2 # box1_pose.position.z = 0.3 # box1_pose_stamped = geometry_msgs.msg.PoseStamped() # box1_pose_stamped.pose = box1_pose # box1_pose_stamped.header = "" # scene.add_box("box1", box1_pose) # rospy.sleep(10) moveit_commander.roscpp_shutdown()
def __init__(self): super(DiffCoplusBaxterExperiments, self).__init__() ## BEGIN_SUB_TUTORIAL setup ## ## First initialize `moveit_commander`_ and a `rospy`_ node: moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('DiffCoplusBaxterExperiments', anonymous=True) ## Instantiate a `RobotCommander`_ object. Provides information such as the robot's ## kinematic model and the robot's current joint states robot = moveit_commander.RobotCommander() ## Instantiate a `PlanningSceneInterface`_ object. This provides a remote interface ## for getting, setting, and updating the robot's internal understanding of the ## surrounding world: scene = moveit_commander.PlanningSceneInterface() ## Instantiate a `MoveGroupCommander`_ object. This object is an interface ## to a planning group (group of joints). In this tutorial the group is the primary ## arm joints in the Panda robot, so we set the group's name to "panda_arm". ## If you are using a different robot, change this value to the name of your robot ## arm planning group. ## This interface can be used to plan and execute motions: group_name = "left_arm" move_group = moveit_commander.MoveGroupCommander(group_name) ## Create a `DisplayTrajectory`_ ROS publisher which is used to display ## trajectories in Rviz: display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) robot_state_publisher = rospy.Publisher('/robot/joint_states', JointState, queue_size=1) self.pos = [0 for _ in range(7)] self.state = JointState() self.state.name = [ 'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1', 'left_w2' ] self.state.position = self.pos self.state_pub_thread = threading.Thread(target=self.pub_state) ## BEGIN_SUB_TUTORIAL basic_info ## ## Getting Basic Information ## ^^^^^^^^^^^^^^^^^^^^^^^^^ # We can get the name of the reference frame for this robot: planning_frame = move_group.get_planning_frame() print("============ Planning frame: %s" % planning_frame) # We can also print the name of the end-effector link for this group: eef_link = move_group.get_end_effector_link() print("============ End effector link: %s" % eef_link) # We can get a list of all the groups in the robot: group_names = robot.get_group_names() print("============ Available Planning Groups:", robot.get_group_names()) # Sometimes for debugging it is useful to print the entire state of the # robot: print("============ Printing robot state") print(robot.get_current_state()) print("") ## END_SUB_TUTORIAL # Misc variables self.box_name = '' self.robot = robot self.scene = scene self.move_group = move_group self.display_trajectory_publisher = display_trajectory_publisher self.robot_state_publisher = robot_state_publisher self.planning_frame = planning_frame self.eef_link = eef_link self.group_names = group_names self.state_pub_thread.start()
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) # Connect to the right_arm move group right_arm = moveit_commander.MoveGroupCommander('arm') # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Display the name of the end_effector link rospy.loginfo("The end effector link is: " + str(end_effector_link)) # Set a small tolerance on joint angles right_arm.set_goal_tolerance(0.0001) right_arm.set_named_target('right') right_arm.go() rospy.sleep(1) ''' start_pose = right_arm.get_current_pose().pose start_pose.position.x = 0.4 start_pose.position.y = 0 #start_pose.position.z = 0.4 start_pose.orientation.x = 0.707106781186547 start_pose.orientation.y = 0 start_pose.orientation.z = 0 start_pose.orientation.w = 0.707106781186547 right_arm.set_pose_target(start_pose) right_arm.go() # Pause for a moment rospy.sleep(1) ''' plan = RobotTrajectory() joint_trajectory = JointTrajectory() joint_trajectory.header.frame_id = 'base_link' #joint_trajectory.header.stamp = rospy.Time.now() joint_trajectory.joint_names = [ 'base_to_armA', 'armA_to_armB', 'armB_to_armC', 'armC_to_armD' ] # Output with open("/home/jyk/Desktop/pvt.txt", 'r') as f: for line in f.readlines(): cont = line.rstrip('\r\n').replace(' ', '').split(',') point = JointTrajectoryPoint() point.positions = map(float, cont[0:4]) point.velocities = map(float, cont[4:8]) point.time_from_start = rospy.Duration(float(cont[8])) joint_trajectory.points.append(deepcopy(point)) plan.joint_trajectory = joint_trajectory #print type(plan) #print plan right_arm.set_start_state_to_current_state() right_arm.execute(plan) #rospy.sleep(3) ''' # Return the arm to the named "resting" pose stored in the SRDF file right_arm.set_named_target('right') right_arm.go() rospy.sleep(1) ''' # Cleanly shut down MoveIt moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
def main(): try: #rospy.init_node('avalos_limb_py') #moveit_commander.roscpp_initialize(sys.argv) moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('move_group_python_interface_tutorial', anonymous=True) robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() group_name = 'right_arm' group = moveit_commander.MoveGroupCommander(group_name) display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) # We can get the name of the reference frame for this robot: planning_frame = group.get_planning_frame() #frecuency for Sawyer robot f = 100 rate = rospy.Rate(f) # add gripper if (False): gripper = intera_interface.Gripper('right_gripper') gripper.calibrate() gripper.open() moveit_robot_state = RobotState() joint_state = JointState() joint_state.header = Header() joint_state.header.stamp = rospy.Time.now() joint_state.name = [ 'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6' ] #Define topic pub = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size=10) # Class to record #data=Data() #Class limb to acces information sawyer limb = Limb() limb.move_to_neutral() group.clear_pose_targets() group.set_start_state_to_current_state() # We can get the joint values from the group and adjust some of the values: pose_goal = geometry_msgs.msg.Pose() # Orientation pose_goal.orientation.x = -1 pose_goal.orientation.y = 0.0 pose_goal.orientation.z = 0.0 pose_goal.orientation.w = 0.0 q0 = np.array([]) q1 = np.array([]) q2 = np.array([]) q3 = np.array([]) q4 = np.array([]) q5 = np.array([]) q6 = np.array([]) # Cartesian position pose_goal.position.x = -0.1 pose_goal.position.y = 0.35 pose_goal.position.z = 0.05 pose_goal = Pose(Point(-0.1, 0.6, 0.05), Quaternion(-1, 0, 0, 0)) group.set_pose_target(pose_goal) a = group.plan() points = a.joint_trajectory.points n = len(points) joint_state.position = [ points[n - 1].positions[0], points[n - 1].positions[1], points[n - 1].positions[2], points[n - 1].positions[3], points[n - 1].positions[4], points[n - 1].positions[5], points[n - 1].positions[6] ] moveit_robot_state.joint_state = joint_state group.set_start_state(moveit_robot_state) for i in range(n): q0 = np.append(q0, points[i].positions[0]) q1 = np.append(q1, points[i].positions[1]) q2 = np.append(q2, points[i].positions[2]) q3 = np.append(q3, points[i].positions[3]) q4 = np.append(q4, points[i].positions[4]) q5 = np.append(q5, points[i].positions[5]) q6 = np.append(q6, points[i].positions[6]) print "q000", q0 # Cartesian position pose_goal.position.x = 0.43 pose_goal.position.y = -0.4 pose_goal.position.z = 0.2 group.set_pose_target(pose_goal) a = group.plan() points = a.joint_trajectory.points n = len(points) joint_state.position = [ points[n - 1].positions[0], points[n - 1].positions[1], points[n - 1].positions[2], points[n - 1].positions[3], points[n - 1].positions[4], points[n - 1].positions[5], points[n - 1].positions[6] ] moveit_robot_state.joint_state = joint_state group.set_start_state(moveit_robot_state) for i in range( n - 1): # Si se repite un numero en posicion entra en un bug q0 = np.append(q0, points[i + 1].positions[0]) q1 = np.append(q1, points[i + 1].positions[1]) q2 = np.append(q2, points[i + 1].positions[2]) q3 = np.append(q3, points[i + 1].positions[3]) q4 = np.append(q4, points[i + 1].positions[4]) q5 = np.append(q5, points[i + 1].positions[5]) q6 = np.append(q6, points[i + 1].positions[6]) #''' q = np.array([q0, q1, q2, q3, q4, q5, q6]) print "q001", q0 print q[0] #return 0 alfa = 0.5 start = time.time() opt = Opt_avalos(q, f, alfa) v_time = opt.full_time() j, v, a, jk = generate_path_cub(q, v_time, f) ext = len(j[0, :]) end = time.time() print('Process Time:', end - start) print ext #save_matrix(j,"data_p.txt",f) #save_matrix(v,"data_v.txt",f) #save_matrix(a,"data_a.txt",f) #save_matrix(jk,"data_y.txt",f) v_jk = sqrt(np.mean(np.square(jk))) print("Opt Time:", v_time) print("Min Time:", m_time) #print('Optimizacion:',opt.result()) print('Costo Tiempo:', len(j[0]) / float(100.0)) print('Costo Jerk:', v_jk) # Position init #raw_input('Iniciar_CT_initial_position?') #limb.move_to_joint_positions({"right_j6": ik1[6],"right_j5": ik1[5], "right_j4": ik1[4], "right_j3": ik1[3], "right_j2": #ik1[2],"right_j1": ik1[1],"right_j0": ik1[0]}) raw_input('Iniciar_CT_execute?') #Build message my_msg = JointCommand() # POSITION_MODE my_msg.mode = 4 my_msg.names = [ "right_j0", "right_j1", "right_j2", "right_j3", "right_j4", "right_j5", "right_j6" ] #data.writeon(str(alfa)+"trayectoria.txt") print("Control por trayectoria iniciado.") #time.sleep(0.25) for n in xrange(ext): my_msg.position = [ j[0][n], j[1][n], j[2][n], j[3][n], j[4][n], j[5][n], j[6][n] ] my_msg.velocity = [ v[0][n], v[1][n], v[2][n], v[3][n], v[4][n], v[5][n], v[6][n] ] my_msg.acceleration = [ a[0][n], a[1][n], a[2][n], a[3][n], a[4][n], a[5][n], a[6][n] ] pub.publish(my_msg) rate.sleep() print("Control por trayectoria terminado.") time.sleep(0.25) #data.writeoff() print("Programa terminado.") except rospy.ROSInterruptException: rospy.logerr('Keyboard interrupt detected from the user.')
def __init__(self): # Name this node, it must be unique rospy.init_node('move_arm', anonymous=False) rospy.on_shutdown( self.cleanup ) # Set rospy to execute a shutdown function when exiting self.ready_for_goal = 0 self.ct = 0 # Set up ROS subscriber callback routines rospy.Subscriber("/move_group/status", GoalStatusArray, self.cb_stat, queue_size=1) rospy.Subscriber("/move_arm/goal", Twist, self.cb_goal, queue_size=1) #rospy.Subscriber("/move_arm/joint_states",JointState, self.cb_joint, queue_size=1) # Set up ROS publishers self.joint_pub = rospy.Publisher("/move_arm/joint_states", JointState, queue_size=1) rospy.set_param('arm_prefix', 'ur5_arm_') rospy.set_param('reference_frame', '/base_link') rospy.loginfo("Moving arm to desired position") rospy.sleep(1) # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() # Initialize the move group for the ur5_arm self.arm = moveit_commander.MoveGroupCommander("ur5_arm") # Get the name of the end-effector link self.end_effector_link = self.arm.get_end_effector_link() rospy.loginfo(self.end_effector_link) # Initialize Necessary Variables reference_frame = rospy.get_param("~reference_frame", "/base_link") #reference_frame = "ee_link" # Set the ur5_arm reference frame accordingly self.arm.set_pose_reference_frame(reference_frame) # Allow replanning to increase the odds of a solution self.arm.allow_replanning(True) # Allow some leeway in position (meters) and orientation (radians) self.arm.set_goal_position_tolerance(0.0001) self.arm.set_goal_orientation_tolerance(0.001) # Set the target pose from the input self.target_pose = PoseStamped() self.target_pose.header.frame_id = reference_frame # Get current joint position to use for planning rospy.loginfo("Initializing motion planning server.") rospy.loginfo("Listening for <Twist> published on </move_arm/goal>")
def __init__(self): """ Setup ROS communication between the Kinect and Baxter :return: """ # Initialise variables self.rgb_img = None self.depth_img = None self.marker_box = None self.marker_center_x = None self.marker_center_y = None self.marker_depth = None self.bridge = CvBridge() # First initialize moveit_commander and rospy. print "============ Initialising Baxter" moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('can_node', anonymous=True) self.robot = moveit_commander.RobotCommander() self.scene = moveit_commander.PlanningSceneInterface() self.left_arm = moveit_commander.MoveGroupCommander("left_arm") self.right_arm = moveit_commander.MoveGroupCommander("right_arm") self.right_arm_navigator = Navigator('right') # Setup grippers self.right_gripper = baxter_interface.Gripper('right') # Setup the table in the scene scene = PlanningSceneInterface() self.scene_pub = rospy.Publisher( '/move_group/monitored_planning_scene', PlanningScene) table_id = 'table' scene.remove_world_object(table_id) rospy.sleep(1) table_ground = -0.3 table_size = [4.0, 4.6, 0.1] table_pose = PoseStamped() table_pose.header.frame_id = self.robot.get_planning_frame() table_pose.pose.position.x = 0.7 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) # Create a dictionary to hold object colors self.colors = dict() self.setColor(table_id, 0.8, 0, 0, 1.0) self.sendColors() self.display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=10) # We can get a list of all the groups in the robot print "============ Right Pose:" print self.right_arm.get_current_pose() print "============ Left Pose:" print self.left_arm.get_current_pose() # Setup the subscribers and publishers self.rgb_sub = rospy.Subscriber("/camera/rgb/image_rect_color", Image, self.callback_rgb) self.points_sub = rospy.Subscriber("/camera/depth_registered/points", PointCloud2, self.callback_points) self.display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=5) self.screen_pub = rospy.Publisher('robot/xdisplay', Image, latch=True, queue_size=10) self.right_hand_range_pub = rospy.Subscriber( "/robot/range/right_hand_range/state", Range, self.callback_range, queue_size=1) self.left_cam_sub = rospy.Subscriber("/cameras/left_hand_camera/image", Image, self.callback_left_hand)
def move_group_python_interface_tutorial(): ## BEGIN_TUTORIAL ## ## Setup ## ^^^^^ ## CALL_SUB_TUTORIAL imports ## ## First initialize moveit_commander and rospy. print "============ Starting tutorial setup" moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('move_group_python_interface_tutorial', anonymous=True) ## Instantiate a RobotCommander object. This object is an interface to ## the robot as a whole. robot = moveit_commander.RobotCommander() ## 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. group = moveit_commander.MoveGroupCommander("left_arm") ## 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, queue_size=20) ## Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up. print "============ Waiting for RVIZ..." rospy.sleep(10) print "============ Starting tutorial " ## Getting Basic Information ## ^^^^^^^^^^^^^^^^^^^^^^^^^ ## ## We can get the name of the reference frame for this robot print "============ Reference frame: %s" % group.get_planning_frame() ## We can also print the name of the end-effector link for this group print "============ Reference frame: %s" % group.get_end_effector_link() ## We can get a list of all the groups in the robot print "============ Robot Groups:" print robot.get_group_names() ## Sometimes for debugging it is useful to print the entire state of the ## robot. print "============ Printing robot state" print robot.get_current_state() print "============" ## Planning to a Pose goal ## ^^^^^^^^^^^^^^^^^^^^^^^ ## We can plan a motion for this group to a desired pose for the ## end-effector print "============ Generating plan 1" pose_target = geometry_msgs.msg.Pose() pose_target.orientation.w = 1.0 pose_target.position.x = 0.7 pose_target.position.y = -0.05 pose_target.position.z = 1.1 group.set_pose_target(pose_target) ## Now, we call the planner to compute the plan ## and visualize it if successful ## Note that we are just planning, not asking move_group ## to actually move the robot plan1 = group.plan() print "============ Waiting while RVIZ displays plan1..." rospy.sleep(5) ## You can ask RVIZ to visualize a plan (aka trajectory) for you. But the ## group.plan() method does this automatically so this is not that useful ## here (it just displays the same trajectory again). print "============ Visualizing plan1" display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = robot.get_current_state() display_trajectory.trajectory.append(plan1) display_trajectory_publisher.publish(display_trajectory); print "============ Waiting while plan1 is visualized (again)..." rospy.sleep(5) ## Moving to a pose goal ## ^^^^^^^^^^^^^^^^^^^^^ ## ## Moving to a pose goal is similar to the step above ## except we now use the go() function. Note that ## the pose goal we had set earlier is still active ## and so the robot will try to move to that goal. We will ## not use that function in this tutorial since it is ## a blocking function and requires a controller to be active ## and report success on execution of a trajectory. # Uncomment below line when working with a real robot # group.go(wait=True) # Use execute instead if you would like the robot to follow # the plan that has already been computed # group.execute(plan1) ## Planning to a joint-space goal ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ ## ## Let's set a joint space goal and move towards it. ## First, we will clear the pose target we had just set. group.clear_pose_targets() ## Then, we will get the current set of joint values for the group group_variable_values = group.get_current_joint_values() print "============ Joint values: ", group_variable_values ## Now, let's modify one of the joints, plan to the new joint ## space goal and visualize the plan group_variable_values[0] = 1.0 group.set_joint_value_target(group_variable_values) plan2 = group.plan() print "============ Waiting while RVIZ displays plan2..." rospy.sleep(5) ## Cartesian Paths ## ^^^^^^^^^^^^^^^ ## You can plan a cartesian path directly by specifying a list of waypoints ## for the end-effector to go through. waypoints = [] # start with the current pose waypoints.append(group.get_current_pose().pose) # first orient gripper and move forward (+x) wpose = geometry_msgs.msg.Pose() wpose.orientation.w = 1.0 wpose.position.x = waypoints[0].position.x + 0.1 wpose.position.y = waypoints[0].position.y wpose.position.z = waypoints[0].position.z waypoints.append(copy.deepcopy(wpose)) # second move down wpose.position.z -= 0.10 waypoints.append(copy.deepcopy(wpose)) # third move to the side wpose.position.y += 0.05 waypoints.append(copy.deepcopy(wpose)) ## We want the cartesian path to be interpolated at a resolution of 1 cm ## which is why we will specify 0.01 as the eef_step in cartesian ## translation. We will specify the jump threshold as 0.0, effectively ## disabling it. (plan3, fraction) = group.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold print "============ Waiting while RVIZ displays plan3..." rospy.sleep(5) # Uncomment the line below to execute this plan on a real robot. # group.execute(plan3) ## Adding/Removing Objects and Attaching/Detaching Objects ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ ## First, we will define the collision object message collision_object = moveit_msgs.msg.CollisionObject() ## When finished shut down moveit_commander. moveit_commander.roscpp_shutdown() ## END_TUTORIAL print "============ STOPPING"
def main(): try: rospy.loginfo("Creating Demo Planning Scene") scene = ExtendedPlanningSceneInterface() rospy.sleep( 1 ) # ----- Not having this delay sometimes caused failing to create some boxes for config in IRLab_workspace: rospy.loginfo("-- Creating object: {}..".format(config['name'])) success = scene.add_box(**config) rospy.loginfo( "------ {}".format("success" if success else "FAILED!")) rospy.loginfo("Created Demo Planning Scene.") except rospy.ROSInterruptException: return except KeyboardInterrupt: return if __name__ == '__main__': rospy.init_node('simple_scene_creator', anonymous=True) moveit_commander.roscpp_initialize(sys.argv) main()
def __init__(self): # Initialize ros and moveit moveit_commander.roscpp_initialize(sys.argv) # Initialize moveit commander self.arm = moveit_commander.MoveGroupCommander('arm') self.scene = moveit_commander.PlanningSceneInterface() # Initialize arm effector link self.end_effector_link = self.arm.get_end_effector_link() # Initialize reference frame self.reference_frame = 'world' self.arm.set_pose_reference_frame(self.reference_frame) # Initialize moveit parameters self.arm.allow_replanning(True) self.arm.set_goal_position_tolerance(0.001) self.arm.set_goal_orientation_tolerance(0.001) self.arm.set_max_velocity_scaling_factor(1.0) #self.set_max_velocity_scaling_factor (0.2) #############添加水平障碍############ rospy.sleep(1) base_table_id = 'base_table' self.scene.remove_world_object(base_table_id) base_table_size = [3, 3, 0.01] base_table_pose = PoseStamped() base_table_pose.header.frame_id = self.reference_frame base_table_pose.pose.position.x = 0 base_table_pose.pose.position.y = 0.0 base_table_pose.pose.position.z = height_shuipin_collision #高度 base_table_pose.pose.orientation.w = 1.0 self.scene.add_box(base_table_id, base_table_pose, base_table_size) rospy.sleep(1) ##############添加后侧障碍############# backward_wall_id = 'backward_wall' self.scene.remove_world_object(backward_wall_id) backward_wall_size = [0.01, 3, 3] backward_wall_pose = PoseStamped() backward_wall_pose.header.frame_id = self.reference_frame backward_wall_pose.pose.position.x = -0.3 backward_wall_pose.pose.position.y = 0.0 backward_wall_pose.pose.position.z = 0.4 backward_wall_pose.pose.orientation.w = 1.0 self.scene.add_box(backward_wall_id, backward_wall_pose, backward_wall_size) rospy.sleep(1) ##############添加中间障碍############# middle_collision_id = 'middle_collision' self.scene.remove_world_object(middle_collision_id) middle_collision_size = [0.22, 0.02, 0.27] middle_collision_pose = PoseStamped() middle_collision_pose.header.frame_id = self.reference_frame middle_collision_pose.pose.position.x = 0.405 middle_collision_pose.pose.position.y = 0.0 middle_collision_pose.pose.position.z = height_shuipin_collision + middle_collision_size[ 2] / 2.0 middle_collision_pose.pose.orientation.w = 1.0 self.scene.add_box(middle_collision_id, middle_collision_pose, middle_collision_size) rospy.sleep(1) ##############添加附着障碍############# tool1_id = 'tool1' self.scene.remove_attached_object(self.end_effector_link, tool1_id) tool1_size = [0.052, 0.13, 0.003] tool1_pose = PoseStamped() tool1_pose.header.frame_id = self.end_effector_link tool1_pose.pose.position.x = 0.0 tool1_pose.pose.position.y = 0.015 tool1_pose.pose.position.z = 0.0 tool1_pose.pose.orientation.w = 1.0 self.scene.attach_box(self.end_effector_link, tool1_id, tool1_pose, tool1_size) rospy.sleep(1) ##############添加附着障碍############# tool2_id = 'tool2' self.scene.remove_attached_object(self.end_effector_link, tool2_id) tool2_size = [0.015, 0.015, 0.18] tool2_pose = PoseStamped() tool2_pose.header.frame_id = self.end_effector_link tool2_pose.pose.position.x = 0.0 tool2_pose.pose.position.y = 0.07 tool2_pose.pose.position.z = 0.04 tool2_pose.pose.orientation.w = 1.0 self.scene.attach_box(self.end_effector_link, tool2_id, tool2_pose, tool2_size) rospy.sleep(1) ##############添加附着障碍############# tool3_id = 'tool3' self.scene.remove_attached_object(self.end_effector_link, tool3_id) tool3_size = [0.09, 0.03, 0.026] tool3_pose = PoseStamped() tool3_pose.header.frame_id = self.end_effector_link tool3_pose.pose.position.x = 0.0 tool3_pose.pose.position.y = -0.035 tool3_pose.pose.position.z = 0.013 tool3_pose.pose.orientation.w = 1.0 self.scene.attach_box(self.end_effector_link, tool3_id, tool3_pose, tool3_size) rospy.sleep(1) #initialize arm position to home self.arm.set_named_target('home') self.arm.go() rospy.sleep(1) try: #a = rospy.ServiceProxy('/set_robot_io', setRobotIo) resp = io_serv(3, True, False) except rospy.ServiceException, e: print "Service call failed: %s" % e
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_obstacles_demo') # Construct the initial scene object scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) # Create a dictionary to hold object colors self.colors = dict() # Pause for the scene to get ready rospy.sleep(1) # Initialize the move group for the right arm arm = MoveGroupCommander(GROUP_NAME_ARM) # Get the name of the end-effector link end_effector_link = arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) arm.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.05) # Allow replanning to increase the odds of a solution arm.allow_replanning(True) # Set the right arm reference frame accordingly arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt arm.set_planning_time(5) # Give each of the scene objects a unique name table_id = 'table' box1_id = 'box1' box2_id = 'box2' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "resting" pose stored in the SRDF file arm.set_named_target('l_arm_init') arm.go() rospy.sleep(2) # Set the height of the table off the ground table_ground = 0.65 # Set the length, width and height of the table and boxes table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # Add a table top and two boxes to the scene table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose.position.x = 0.35 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose = PoseStamped() box1_pose.header.frame_id = REFERENCE_FRAME box1_pose.pose.position.x = 0.3 box1_pose.pose.position.y = 0 box1_pose.pose.position.z = table_ground + table_size[ 2] + box1_size[2] / 2.0 box1_pose.pose.orientation.w = 1.0 scene.add_box(box1_id, box1_pose, box1_size) box2_pose = PoseStamped() box2_pose.header.frame_id = REFERENCE_FRAME box2_pose.pose.position.x = 0.3 box2_pose.pose.position.y = 0.25 box2_pose.pose.position.z = table_ground + table_size[ 2] + box2_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 scene.add_box(box2_id, box2_pose, box2_size) # Make the table red and the boxes orange self.setColor(table_id, 0.8, 0, 0, 1.0) self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the target pose in between the boxes and above the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.22 target_pose.pose.position.y = 0.14 target_pose.pose.position.z = table_pose.pose.position.z + table_size[ 2] + 0.05 q = quaternion_from_euler(0, 0, -1.57079633) target_pose.pose.orientation.x = q[0] target_pose.pose.orientation.y = q[1] target_pose.pose.orientation.z = q[2] target_pose.pose.orientation.w = q[3] # Set the target pose for the arm arm.set_pose_target(target_pose, end_effector_link) # Move the arm to the target pose (if possible) arm.go() # Pause for a moment... rospy.sleep(2) # Return the arm to the "resting" pose stored in the SRDF file arm.set_named_target('l_arm_init') arm.go() # Exit MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Use the planning scene object to add or remove objects scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, 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) right_arm.set_planner_id("RRTConnectkConfigDefault") # 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() print(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.01) # 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 = 2 # Set a limit on the number of place attempts max_place_attempts = 5 # Give the scene a chance to catch up rospy.sleep(2) print(right_arm.get_current_pose()) # clean the scene scene.remove_world_object("table") scene.remove_world_object("part") # Remove any attached objects from a previous session scene.remove_attached_object(GRIPPER_FRAME, "part") # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "grasp" pose stored in the SRDF file right_arm.set_named_target('default') right_arm.go() # Open the gripper to the neutral position right_gripper.set_joint_value_target(GRIPPER_OPEN) right_gripper.go() rospy.sleep(1) place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = 0.11 place_pose.pose.position.y = 0.22 place_pose.pose.position.z = 0.05 scene.add_box("part", place_pose, (0.015, 0.05, 0.015)) # Specify a pose to place the target after being picked up target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME # start the gripper in a neutral pose part way to the target target_pose.pose.position.x = 0.130465574219 target_pose.pose.position.y = 0.171727879517 target_pose.pose.position.z = 0.091324779826 target_pose.pose.orientation.x = -0.929337637511 target_pose.pose.orientation.y = 0.312980256889 target_pose.pose.orientation.z = -0.0625227051047 target_pose.pose.orientation.w = 0.185649739157 # Initialize the grasp pose to the target pose grasp_pose = target_pose print("going to start pose") right_arm.set_pose_target(target_pose) right_arm.go() rospy.sleep(2) # Shift the grasp pose by half the width of the target to center it #grasp_pose.pose.position.y -= target_size[1] / 2.0 #grasp_pose.pose.position.x = 0.12792118579 #grasp_pose.pose.position.y = -0.285290879999 #grasp_pose.pose.position.z = 0.120301181892 # Generate a list of grasps grasps = self.make_grasps(grasp_pose, 'part') # Publish the grasp poses so they can be viewed in RViz print "Publishing grasps" 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('part', grasps) rospy.sleep(0.2) # If the pick was successful, attempt the place operation if result == MoveItErrorCodes.SUCCESS: result = None n_attempts = 0 # Generate valid place poses places = self.make_places(place_pose) # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts: n_attempts += 1 rospy.loginfo("Place attempt: " + str(n_attempts)) for place in places: result = right_arm.place('part', place) if result == MoveItErrorCodes.SUCCESS: break rospy.sleep(0.2) if result != MoveItErrorCodes.SUCCESS: rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.") else: # Return the arm to the "resting" pose stored in the SRDF file right_arm.set_named_target('right_arm_rest') right_arm.go() # Open the gripper to the open position right_gripper.set_joint_value_target(GRIPPER_OPEN) right_gripper.go() else: rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.") rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
def __init__(self, group_name): super(MoveGroupPythonIntefaceTutorial, self).__init__() ## BEGIN_SUB_TUTORIAL setup ## ## First initialize `moveit_commander`_ and a `rospy`_ node: moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('move_group_python_interface_tutorial', anonymous=True) ## Instantiate a `RobotCommander`_ object. This object is the outer-level interface to ## the robot: robot = moveit_commander.RobotCommander() ## 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 Panda ## arm so we set ``group_name = panda_arm``. If you are using a different robot, ## you should change this value to the name of your robot arm planning group. ## This interface can be used to plan and execute motions on the Panda: # group_name = "right_arm" group = moveit_commander.MoveGroupCommander(group_name) ## We create a `DisplayTrajectory`_ publisher which is used later to publish ## trajectories for RViz to visualize: display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) gripper_pub = rospy.Publisher( '/yumi/gripper_effort_controller_l/command', Float64) ## END_SUB_TUTORIAL ## BEGIN_SUB_TUTORIAL basic_info ## ## Getting Basic Information ## ^^^^^^^^^^^^^^^^^^^^^^^^^ # We can get the name of the reference frame for this robot: planning_frame = group.get_planning_frame() print "============ Reference frame: %s" % planning_frame # We can also print the name of the end-effector link for this group: eef_link = group.get_end_effector_link() print "============ End effector: %s" % eef_link # We can get a list of all the groups in the robot: group_names = robot.get_group_names() print "============ Robot Groups:", robot.get_group_names() # Sometimes for debugging it is useful to print the entire state of the # robot: print "============ Printing robot state" print robot.get_current_state() print "" ## END_SUB_TUTORIAL # Misc variables self.box_name = '' self.robot = robot self.scene = scene self.group = group self.display_trajectory_publisher = display_trajectory_publisher self.planning_frame = planning_frame self.eef_link = eef_link self.group_names = group_names self.gripper_pub = gripper_pub
def __init__(self): rospy.init_node("moveit_cartesian_path", anonymous=False) rospy.loginfo("Starting node moveit_cartesian_path") rospy.on_shutdown(self.cleanup) # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) # Initialize the move group for the ur5_arm self.arm = moveit_commander.MoveGroupCommander('manipulator') # Get the name of the end-effector link end_effector_link = self.arm.get_end_effector_link() # Set the reference frame for pose targets reference_frame = "/base_link" # Set the ur5_arm reference frame accordingly self.arm.set_pose_reference_frame(reference_frame) # Allow replanning to increase the odds of a solution self.arm.allow_replanning(True) # Allow some leeway in position (meters) and orientation (radians) self.arm.set_goal_position_tolerance(0.01) self.arm.set_goal_orientation_tolerance(0.1) # Get the current pose so we can add it as a waypoint start_pose = self.arm.get_current_pose(end_effector_link).pose print "start pose", start_pose orientation_list = [ start_pose.orientation.x, start_pose.orientation.y, start_pose.orientation.z, start_pose.orientation.w ] (roll, pitch, yaw) = euler_from_quaternion(orientation_list) print roll, pitch, yaw quat = quaternion_from_euler(roll, pitch, yaw) print quat # Initialize the waypoints list waypoints = [] # Set the first waypoint to be the starting pose # Append the pose to the waypoints list waypoints.append(start_pose) #wpose = deepcopy(start_pose) # Set the next waypoint to the right 0.5 meters #wpose.position.y -= 0.1 #waypoints.append(deepcopy(wpose)) fraction = 0.0 maxtries = 100 attempts = 0 # Set the internal state to the current state self.arm.set_start_state_to_current_state() # Plan the Cartesian path connecting the waypoints while fraction < 1.0 and attempts < maxtries: (plan, fraction) = self.arm.compute_cartesian_path( waypoints, 0.01, 0.0, True) # 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.") self.arm.execute(plan) rospy.loginfo("Path execution complete.") else: rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")
def __init__(self, temp): ## First initialize `moveit_commander`_ and a `rospy`_ node: moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('move_group_python', anonymous=True) ## Instantiate a `RobotCommander`_ object. Provides information such as the robot's ## kinematic model and the robot's current joint states robot = moveit_commander.RobotCommander() ## Instantiate a `PlanningSceneInterface`_ object. This provides a remote interface ## for getting, setting, and updating the robot's internal understanding of the ## surrounding world: scene = moveit_commander.PlanningSceneInterface() ## Instantiate a `MoveGroupCommander`_ object. This object is an interface ## to a planning group (group of joints). In this tutorial the group is the primary ## arm joints in the Panda robot, so we set the group's name to "panda_arm". ## If you are using a different robot, change this value to the name of your robot ## arm planning group. ## This interface can be used to plan and execute motions: group_name = temp move_group = moveit_commander.MoveGroupCommander(group_name) ## Create a `DisplayTrajectory`_ ROS publisher which is used to display ## trajectories in Rviz: display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) ## Getting Basic Information # We can get the name of the reference frame for this robot: planning_frame = move_group.get_planning_frame() #print "============ Planning frame: %s" % planning_frame # We can also print the name of the end-effector link for this group: eef_link = move_group.get_end_effector_link() #print "============ End effector link: %s" % eef_link # We can get a list of all the groups in the robot: group_names = robot.get_group_names() print "============ Available Planning Groups:", robot.get_group_names( ) # Sometimes for debugging it is useful to print the entire state of the # robot: # print "============ Printing robot state" # print robot.get_current_state() # print "" # # print('--------------------------------------------') # ''''Get the current pose of the end-effector of the group.''' # print(move_group.get_current_pose().pose) # Misc variables self.robot = robot self.scene = scene self.move_group = move_group self.display_trajectory_publisher = display_trajectory_publisher self.planning_frame = planning_frame self.eef_link = eef_link self.group_names = group_names self.name_arm = group_name self.tag_name = temp #ok
def handle_req(req): ## First initialize `moveit_commander`_ and a `rospy`_ node: moveit_commander.roscpp_initialize(sys.argv) ## Instantiate a `RobotCommander`_ object. Provides information such as the robot's ## kinematic model and the robot's current joint states robot = moveit_commander.RobotCommander() ## Instantiate a `PlanningSceneInterface`_ object. This provides a remote interface ## for getting, setting, and updating the robot's internal understanding of the ## surrounding world: scene = moveit_commander.PlanningSceneInterface() ## Instantiate a `MoveGroupCommander`_ object. This object is an interface ## to a planning group (group of joints). ## This interface can be used to plan and execute motions: group_name = req.group ## print "============ Robot Groups:" print robot.get_group_names() ## print "============ Printing robot state" print robot.get_current_state() move_group = moveit_commander.MoveGroupCommander(group_name.data) print "Pose is: " print move_group.get_current_pose().pose # The group_thumb has only two valid joints, hence the if-else statement # if the group_thumb is selected, accept only two joint values # else 3 joint values if(group_name.data == "group_thumb"): ## Create a `DisplayTrajectory`_ ROS publisher which is used to display ## trajectories in Rviz: display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) # We can get the joint values from the group and adjust some of the values: joint_goal = move_group.get_current_joint_values() # print Joint values print move_group.get_current_joint_values()[0] print move_group.get_current_joint_values()[1] joint_goal[0] = req.joint_goal1.data joint_goal[1] = req.joint_goal2.data # The go command can be called with joint values, poses, or without any # parameters if you have already set the pose or joint target for the group move_group.go(joint_goal, wait=True) # Calling ``stop()`` ensures that there is no residual movement move_group.stop() # For testing: current_joints = move_group.get_current_joint_values() # print Joint values print move_group.get_current_joint_values()[0] print move_group.get_current_joint_values()[1] else: ## Instantiate a `MoveGroupCommander`_ object. This object is an interface ## to a planning group (group of joints). ## This interface can be used to plan and execute motions: move_group = moveit_commander.MoveGroupCommander(group_name.data) ## Create a `DisplayTrajectory`_ ROS publisher which is used to display ## trajectories in Rviz: display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) # We can get the joint values from the group and adjust some of the values: joint_goal = move_group.get_current_joint_values() # The go command can be called with joint values, poses, or without any # parameters if you have already set the pose or joint target for the group # print Joint values print move_group.get_current_joint_values()[0] print move_group.get_current_joint_values()[1] print move_group.get_current_joint_values()[2] joint_goal[0] = req.joint_goal1.data joint_goal[1] = req.joint_goal2.data joint_goal[2] = req.joint_goal3.data # The go command can be called with joint values, poses, or without any # parameters if you have already set the pose or joint target for the group move_group.go(joint_goal, wait=True) # Calling ``stop()`` ensures that there is no residual movement move_group.stop() # For testing: current_joints = move_group.get_current_joint_values() # print Joint values print move_group.get_current_joint_values()[0] print move_group.get_current_joint_values()[1] print move_group.get_current_joint_values()[2] rep = udm_serviceResponse() rep.res = True rep.message = "This action was planned at %s" %rospy.get_time() return rep
def __init__(self): super(PickAndDropProgram, self).__init__() # First initialize `moveit_commander`_ and a `rospy`_ node: moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('pick_and_drop_program', anonymous=True) filepath = rospy.get_param('~goal_joint_states') goal_joint_states = joint_state_filereader.read(filepath) self.goal_names = [ "home", "near_box", "near_drop", "drop", "pick" ] for name in self.goal_names: assert name in goal_joint_states, \ "Joint state name '{}' is not found".format(name) # Declare suctionpad topics self.pub_to = rospy.Publisher('toArduino', String, queue_size=100) self.pub_from = rospy.Publisher('fromArduino', String, queue_size=100) # Instantiate a `RobotCommander`_ object. This object is the outer-level interface to # the robot: robot = moveit_commander.RobotCommander() # 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 UR5 # arm so we set ``group_name = manipulator``. If you are using a different robot, # you should change this value to the name of your robot arm planning group. group_name = "manipulator" # See .srdf file to get available group names group = moveit_commander.MoveGroupCommander(group_name) # We create a `DisplayTrajectory`_ publisher which is used later to publish # trajectories for RViz to visualize: # display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', # moveit_msgs.msg.DisplayTrajectory, # queue_size=20) # We can get the name of the reference frame for this robot: planning_frame = group.get_planning_frame() print("============ Reference frame: %s" % planning_frame) # We can also print(the name of the end-effector link for this group: eef_link = group.get_end_effector_link() print("============ End effector: %s" % eef_link) # We can get a list of all the groups in the robot: group_names = robot.get_group_names() print("============ Robot Groups:", robot.get_group_names()) # Sometimes for debugging it is useful to print the entire state of the # robot: print("============ Printing robot state") print(robot.get_current_state()) print("") # Misc variables self.robot = robot self.scene = scene self.group = group self.group_names = group_names self.goal_joint_states = dict(goal_joint_states)
def __init__(self): # Initialize move_group API moveit_commander.roscpp_initialize(sys.argv) # Initialize ROS node rospy.init_node('moveit_obstacles_demo') # Initialize scene scene = PlanningSceneInterface() # Create a publisher for scene change information self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) # Create a dictionary to store object color self.colors = dict() # Wait for scene to be ready rospy.sleep(1) # Initialize arm_group which needs to be controlled by move_group arm = MoveGroupCommander('arm') # Get end link name end_effector_link = arm.get_end_effector_link() # Set tolerance for position (m) and orientation (rad) arm.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.05) # Allow replanning if failed arm.allow_replanning(True) # Set reference frame for target position reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) # Set planning time limitation arm.set_planning_time(5) # Set scene object name front_board_id = 'front_board' back_board_id = 'back_board' left_board_id = 'left_board' right_board_id = "right_board" bottle_id = 'bottle' # Remove previous object scene.remove_world_object(front_board_id) scene.remove_world_object(back_board_id) scene.remove_world_object(left_board_id) scene.remove_world_object(right_board_id) scene.remove_world_object(bottle_id) rospy.sleep(1) # Move arm to initial position arm.set_named_target('home') arm.go() rospy.sleep(2) # Initialize gripper_group which needs to be controlled by move_group gripper = moveit_commander.MoveGroupCommander('gripper') # Set object dimension front_board_size = [0.2, 0.25, 0.01] back_board_size = [0.2, 0.25, 0.01] left_board_size = [0.25, 0.14, 0.01] right_board_size = [0.25, 0.14, 0.01] bottle_height, bottle_radius = 0.06, 0.02 # Add object to scene front_board_pose = PoseStamped() front_board_pose.header.frame_id = reference_frame front_board_pose.pose.position.x = 0 front_board_pose.pose.position.y = 0.1 front_board_pose.pose.position.z = 0.125 front_board_pose.pose.orientation.w = 0.707 front_board_pose.pose.orientation.x = 0.707 scene.add_box(front_board_id, front_board_pose, front_board_size) back_board_pose = PoseStamped() back_board_pose.header.frame_id = reference_frame back_board_pose.pose.position.x = 0 back_board_pose.pose.position.y = 0.24 back_board_pose.pose.position.z = 0.125 back_board_pose.pose.orientation.w = 0.707 back_board_pose.pose.orientation.x = 0.707 scene.add_box(back_board_id, back_board_pose, back_board_size) left_board_pose = PoseStamped() left_board_pose.header.frame_id = reference_frame left_board_pose.pose.position.x = 0.1 left_board_pose.pose.position.y = 0.17 left_board_pose.pose.position.z = 0.125 left_board_pose.pose.orientation.w = 0.707 left_board_pose.pose.orientation.y = 0.707 scene.add_box(left_board_id, left_board_pose, left_board_size) right_board_pose = PoseStamped() right_board_pose.header.frame_id = reference_frame right_board_pose.pose.position.x = -0.1 right_board_pose.pose.position.y = 0.17 right_board_pose.pose.position.z = 0.125 right_board_pose.pose.orientation.w = 0.707 right_board_pose.pose.orientation.y = 0.707 scene.add_box(right_board_id, right_board_pose, right_board_size) bottle_pose = PoseStamped() bottle_pose.header.frame_id = reference_frame bottle_pose.pose.position.x = 0.1 bottle_pose.pose.position.y = -0.2 bottle_pose.pose.position.z = 0.02 bottle_pose.pose.orientation.w = 0.707 bottle_pose.pose.orientation.y = 0.707 scene.add_cylinder(bottle_id, bottle_pose, bottle_height, bottle_radius) # Set object color self.setColor(front_board_id, 1.0, 1.0, 1.0, 0.6) self.setColor(back_board_id, 1.0, 1.0, 1.0, 0.6) self.setColor(left_board_id, 1.0, 1.0, 1.0, 0.6) self.setColor(right_board_id, 1.0, 1.0, 1.0, 0.6) self.setColor(bottle_id, 0.6, 0.1, 0, 1.0) # Publish color self.sendColors() rospy.sleep(3) # Set target position for gripper and move gripper gripper_position = (np.array([36, -36, 36, 36]) * np.pi / 180.0).tolist() gripper.set_joint_value_target(gripper_position) gripper.go() rospy.sleep(5) # Set target position for arm joint_position = (np.array([28, -10, 50, -88, 58]) * np.pi / 180.0).tolist() arm.set_joint_value_target(joint_position) arm.go() # Set target position for arm joint_position = (np.array([28, -44, 30, -91, 58]) * np.pi / 180.0).tolist() arm.set_joint_value_target(joint_position) arm.go() # Set target position for gripper and move gripper gripper_position = (np.array([20, -20, 20, 20]) * np.pi / 180.0).tolist() gripper.set_joint_value_target(gripper_position) gripper.go() rospy.sleep(10) # Move arm to initial position arm.set_named_target('home') arm.go() # Set target position for gripper and move gripper gripper_position = (np.array([-33, 33, -33, -33]) * np.pi / 180.0).tolist() gripper.set_joint_value_target(gripper_position) gripper.go() # Close and exit MoveIt moveit_commander.roscpp_shutdown()
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_ik') # Initialize the move group for the right arm arm = moveit_commander.MoveGroupCommander('arm') # Get the name of the end-effector link end_effector_link = arm.get_end_effector_link() print end_effector_link # Set the reference frame for pose targets reference_frame = '/base_link' # Set the arm reference frame accordingly arm.set_pose_reference_frame(reference_frame) # 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.002) arm.set_goal_orientation_tolerance(3.14) # Start the arm in the "resting" pose stored in the SRDF file #arm.set_named_target('resting') #arm.go() #rospy.sleep(2) # Test forward kinematics joint_positions = [0.0, 0.0, 0.0, 0.0, 0.0] arm.set_joint_value_target(joint_positions) traj = arm.plan() arm.execute(traj) # LeeChan tpose = arm.get_current_pose() print type(tpose) print tpose # Set the target pose. This particular pose has the gripper oriented horizontally # 0.85 meters above the ground, 0.10 meters to the right and 0.20 meters ahead of # the center of the robot base. target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.header.stamp = rospy.Time.now() target_pose.pose.position.x = 0.25515 target_pose.pose.position.y = -0.255147 target_pose.pose.position.z = 0.510048 #target_pose.pose.orientation.x = 0.034 #target_pose.pose.orientation.y = 0.673 #target_pose.pose.orientation.z = 0.0 target_pose.pose.orientation.w = 1 print type(target_pose) print target_pose # Set the start state to the current state arm.set_start_state_to_current_state() # Set the goal pose of the end effector to the stored pose arm.set_pose_target(target_pose, end_effector_link) # Plan the trajectory to the goal traj = arm.plan() # Execute the planned trajectory arm.execute(traj) # Pause for a second rospy.sleep(1) ############################################### target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.header.stamp = rospy.Time.now() target_pose.pose.position.x = 0.25305 target_pose.pose.position.y = -0.1963 target_pose.pose.position.z = 0.6633 #target_pose.pose.orientation.x = 0.034 #target_pose.pose.orientation.y = 0.673 #target_pose.pose.orientation.z = 0.0 target_pose.pose.orientation.w = 1 print type(target_pose) print target_pose arm.set_start_state_to_current_state() arm.set_pose_target(target_pose, end_effector_link) traj = arm.plan() arm.execute(traj) rospy.sleep(1) ################################################# # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit MoveIt moveit_commander.os._exit(0)