def createExecuteKnownTrajectoryRequest(trajectory, wait_for_execution=True): """Create a ExecuteKnownTrajectoryRequest from the given data, trajectory must be a RobotTrajectory probably filled from a GetCartesianPath call""" ektr = ExecuteKnownTrajectoryRequest() ektr.trajectory = trajectory ektr.wait_for_execution = wait_for_execution return ektr
def createExecuteKnownTrajectoryRequest(trajectory, wait_for_execution=True): """Create a ExecuteKnownTrajectoryRequest from the given data, trajectory must be a RobotTrajectory probably filled from a GetCartesianPath call""" ektr = ExecuteKnownTrajectoryRequest() ektr.trajectory = trajectory ektr.wait_for_execution = wait_for_execution return ektr
def execute_trajectory(trajectory, wait_for_execution=True): req = ExecuteKnownTrajectoryRequest() req.trajectory = RobotTrajectory() req.trajectory.joint_trajectory = trajectory req.wait_for_execution = wait_for_execution srv = rospy.ServiceProxy('/execute_kinematic_path', ExecuteKnownTrajectory) res = srv(req) if wait_for_execution: rospy.sleep(1) return res.error_code
def execute(trajectory, wait=True, persistent=False, wait_timeout=None): """Call set robot speed service INPUT trajectory [RobotTrajectory] wait [Bool] persistent [Bool, default=False] wait_timeout [Float, default=None] OUTPUT response [SetSpeedResponse] """ srv_proxy = get_service_proxy(SRV_EXECUTE, ExecuteKnownTrajectory, persistent, wait_timeout) req = ExecuteKnownTrajectoryRequest() req.trajectory = trajectory req.wait_for_execution = wait res = srv_proxy(req) return res
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("right_arm") #group.set_planner_id("PRMkConfigDefault") ## 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) ## 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 "============" rospy.sleep(5) print "Going to compute a cartesian path from where we are to z + 0.3" ## 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 rospy.sleep(1) 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 wpose.position.y = waypoints[0].position.y wpose.position.z = waypoints[0].position.z + 0.15 waypoints.append(copy.deepcopy(wpose)) # fourth move to the side a lot wpose.position.z += 0.15 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. fraction = 0.0 jump_threshold = 0 eef_step = 0.01 while fraction < 0.9: (plan3, fraction) = group.compute_cartesian_path( waypoints, # waypoints to follow eef_step, # eef_step jump_threshold) # jump_threshold print "Fraction is: " + str(fraction) + " if it's less than 0.9 we will continue calculating" print "changing jump_threshold + 10 it was: " + str(jump_threshold) print "(Seems to make no difference)" jump_threshold += 10 print "eef-step is: " + str(eef_step) + " adding 1cm" eef_step += 0.01 print "plan looks like: " + str(plan3) print "with fraction being: " + str(fraction) print "Fraction should be 1, less than that, is that we went out of the straight line, i think" print "Moving arm to cartesian path thing" #group.go(wait=True) rospy.sleep(1) print "this did for sure not work, so lets try with executeknowntrajectory thing" ekp = rospy.ServiceProxy('/execute_kinematic_path', ExecuteKnownTrajectory) ekp.wait_for_service() print "!!!! Gonna send goal to execute_kinematic_path (waiting 3s)" rospy.sleep(3) ektr = ExecuteKnownTrajectoryRequest() ektr.trajectory = plan3 ektr.wait_for_execution = True print "Sending call" ekp.call(ektr) print "!!!! Call done" ## 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 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("right_arm") #group.set_planner_id("PRMkConfigDefault") ## 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) ## 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 to initial pose" pose_target = geometry_msgs.msg.Pose() pose_target.orientation.w = 1.0 pose_target.position.x = 0.3 pose_target.position.y = -0.3 pose_target.position.z = 1.1 group.set_pose_target(pose_target) plan1 = group.plan() # Uncomment below line when working with a real robot print "Moving arm to initial 3d pose: " + str(pose_target) group.go(wait=True) rospy.sleep(1) group.clear_pose_targets() print "============ Generating plan to final pose" pose_target = geometry_msgs.msg.Pose() pose_target.orientation.w = 1.0 pose_target.position.x = 0.3 pose_target.position.y = -0.3 pose_target.position.z = 1.4 group.set_pose_target(pose_target) plan1 = group.plan() # Uncomment below line when working with a real robot print "Moving arm to final pose: " + str(pose_target) group.go(wait=True) rospy.sleep(1) group.clear_pose_targets() print "If everything worked well here, then we should be able to do a straight path" print "============ Generating plan to initial pose" pose_target = geometry_msgs.msg.Pose() pose_target.orientation.w = 1.0 pose_target.position.x = 0.3 pose_target.position.y = -0.3 pose_target.position.z = 1.1 group.set_pose_target(pose_target) plan1 = group.plan() # Uncomment below line when working with a real robot print "Moving arm to initial 3d pose: " + str(pose_target) group.go(wait=True) rospy.sleep(1) group.clear_pose_targets() ## 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 rospy.sleep(1) 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 wpose.position.y = waypoints[0].position.y wpose.position.z = waypoints[0].position.z + 0.15 waypoints.append(copy.deepcopy(wpose)) # fourth move to the side a lot wpose.position.z += 0.15 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.02, # eef_step 0.0) # jump_threshold print "============ Waiting while RVIZ displays plan3..." rospy.sleep(3) print "plan3 looks like: " + str(plan3) print "with fraction being: " + str(fraction) print "Moving arm to cartesian path thing" group.go(wait=True) print "this did for sure not work, so lets try with executeknowntrajectory thing" ekp = rospy.ServiceProxy('/execute_kinematic_path', ExecuteKnownTrajectory) ekp.wait_for_service() print "!!!! Gonna send goal to execute_kinematic_path" rospy.sleep(4) ektr = ExecuteKnownTrajectoryRequest() ektr.trajectory = plan3 ektr.wait_for_execution = True ekp.call(ektr) print "!!!! Call done" ## 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 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. right_group = moveit_commander.MoveGroupCommander("right_arm") left_group = moveit_commander.MoveGroupCommander("left_arm") #group.set_planner_id("PRMkConfigDefault") print "Going to compute a cartesian path for right arm from where we are to z + 0.3" ### Right part waypoints = [] # start with the current pose rospy.sleep(1) waypoints.append(right_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 wpose.position.y = waypoints[0].position.y wpose.position.z = waypoints[0].position.z + 0.15 waypoints.append(copy.deepcopy(wpose)) # fourth move to the side a lot wpose.position.z += 0.15 waypoints.append(copy.deepcopy(wpose)) (planright, fraction) = right_group.compute_cartesian_path( waypoints, # waypoints to follow 0.02, # eef_step 0.0) # jump_threshold print "planright looks like: " + str(planright) print "with fraction being: " + str(fraction) #### left part waypoints = [] # start with the current pose rospy.sleep(1) waypoints.append(left_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 wpose.position.y = waypoints[0].position.y wpose.position.z = waypoints[0].position.z + 0.15 waypoints.append(copy.deepcopy(wpose)) # fourth move to the side a lot wpose.position.z += 0.15 waypoints.append(copy.deepcopy(wpose)) (planleft, fraction) = left_group.compute_cartesian_path( waypoints, # waypoints to follow 0.02, # eef_step 0.0) # jump_threshold print "planleft looks like: " + str(planleft) print "with fraction being: " + str(fraction) ### EXECUTE STUFF rospy.sleep(1) finalplan = join_trajectories(planright, planleft) print "final plan looks like: " + str(finalplan) print "so lets try with executeknowntrajectory thing" ekp = rospy.ServiceProxy('/execute_kinematic_path', ExecuteKnownTrajectory) ekp.wait_for_service() print "!!!! Gonna send BOTH ARMS goal to execute_kinematic_path" #rospy.sleep(3) ektr = ExecuteKnownTrajectoryRequest() ektr.trajectory = finalplan ektr.wait_for_execution = True print "Sending call both arms" ekp.call(ektr) print "!!!! Call done" # # print "!!!! Gonna send LEFT goal to execute_kinematic_path" # #rospy.sleep(3) # ektr = ExecuteKnownTrajectoryRequest() # ektr.trajectory = planleft # ektr.wait_for_execution = True # print "Sending call left arm" # ekp.call(ektr) # print "!!!! Call done" ## When finished shut down moveit_commander. moveit_commander.roscpp_shutdown() ## END_TUTORIAL print "============ STOPPING"
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. right_group = moveit_commander.MoveGroupCommander("right_arm") left_group = moveit_commander.MoveGroupCommander("left_arm") #group.set_planner_id("PRMkConfigDefault") print "Going to compute a cartesian path for right arm from where we are to z + 0.3" ### Right part waypoints = [] # start with the current pose rospy.sleep(1) waypoints.append(right_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 wpose.position.y = waypoints[0].position.y wpose.position.z = waypoints[0].position.z + 0.15 waypoints.append(copy.deepcopy(wpose)) # fourth move to the side a lot wpose.position.z += 0.15 waypoints.append(copy.deepcopy(wpose)) (planright, fraction) =right_group.compute_cartesian_path( waypoints, # waypoints to follow 0.02, # eef_step 0.0) # jump_threshold print "planright looks like: " + str(planright) print "with fraction being: " + str(fraction) #### left part waypoints = [] # start with the current pose rospy.sleep(1) waypoints.append(left_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 wpose.position.y = waypoints[0].position.y wpose.position.z = waypoints[0].position.z + 0.15 waypoints.append(copy.deepcopy(wpose)) # fourth move to the side a lot wpose.position.z += 0.15 waypoints.append(copy.deepcopy(wpose)) (planleft, fraction) = left_group.compute_cartesian_path( waypoints, # waypoints to follow 0.02, # eef_step 0.0) # jump_threshold print "planleft looks like: " + str(planleft) print "with fraction being: " + str(fraction) ### EXECUTE STUFF rospy.sleep(1) finalplan = join_trajectories(planright, planleft) print "final plan looks like: " + str(finalplan) print "so lets try with executeknowntrajectory thing" ekp = rospy.ServiceProxy('/execute_kinematic_path', ExecuteKnownTrajectory) ekp.wait_for_service() print "!!!! Gonna send BOTH ARMS goal to execute_kinematic_path" #rospy.sleep(3) ektr = ExecuteKnownTrajectoryRequest() ektr.trajectory = finalplan ektr.wait_for_execution = True print "Sending call both arms" ekp.call(ektr) print "!!!! Call done" # # print "!!!! Gonna send LEFT goal to execute_kinematic_path" # #rospy.sleep(3) # ektr = ExecuteKnownTrajectoryRequest() # ektr.trajectory = planleft # ektr.wait_for_execution = True # print "Sending call left arm" # ekp.call(ektr) # print "!!!! Call done" ## When finished shut down moveit_commander. moveit_commander.roscpp_shutdown() ## END_TUTORIAL print "============ STOPPING"
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("right_arm") #group.set_planner_id("PRMkConfigDefault") ## 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) ## 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 to initial pose" pose_target = geometry_msgs.msg.Pose() pose_target.orientation.w = 1.0 pose_target.position.x = 0.3 pose_target.position.y = -0.3 pose_target.position.z = 1.1 group.set_pose_target(pose_target) plan1 = group.plan() # Uncomment below line when working with a real robot print "Moving arm to initial 3d pose: " + str(pose_target) group.go(wait=True) rospy.sleep(1) group.clear_pose_targets() print "============ Generating plan to final pose" pose_target = geometry_msgs.msg.Pose() pose_target.orientation.w = 1.0 pose_target.position.x = 0.3 pose_target.position.y = -0.3 pose_target.position.z = 1.4 group.set_pose_target(pose_target) plan1 = group.plan() # Uncomment below line when working with a real robot print "Moving arm to final pose: " + str(pose_target) group.go(wait=True) rospy.sleep(1) group.clear_pose_targets() print "If everything worked well here, then we should be able to do a straight path" print "============ Generating plan to initial pose" pose_target = geometry_msgs.msg.Pose() pose_target.orientation.w = 1.0 pose_target.position.x = 0.3 pose_target.position.y = -0.3 pose_target.position.z = 1.1 group.set_pose_target(pose_target) plan1 = group.plan() # Uncomment below line when working with a real robot print "Moving arm to initial 3d pose: " + str(pose_target) group.go(wait=True) rospy.sleep(1) group.clear_pose_targets() ## 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 rospy.sleep(1) 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 wpose.position.y = waypoints[0].position.y wpose.position.z = waypoints[0].position.z + 0.15 waypoints.append(copy.deepcopy(wpose)) # fourth move to the side a lot wpose.position.z += 0.15 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.02, # eef_step 0.0) # jump_threshold print "============ Waiting while RVIZ displays plan3..." rospy.sleep(3) print "plan3 looks like: " + str(plan3) print "with fraction being: " + str(fraction) print "Moving arm to cartesian path thing" group.go(wait=True) print "this did for sure not work, so lets try with executeknowntrajectory thing" ekp = rospy.ServiceProxy('/execute_kinematic_path', ExecuteKnownTrajectory) ekp.wait_for_service() print "!!!! Gonna send goal to execute_kinematic_path" rospy.sleep(4) ektr = ExecuteKnownTrajectoryRequest() ektr.trajectory = plan3 ektr.wait_for_execution = True ekp.call(ektr) print "!!!! Call done" ## 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"