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 on_enter(self, userdata): self._failed = False request = ExecuteKnownTrajectoryRequest() request.trajectory.joint_trajectory = userdata.joint_trajectory request.wait_for_execution = True try: self._result = self._srv.call(self._topic, request) except Exception as e: Logger.logwarn("Was unable to execute joint trajectory:\n%s" % str(e)) self._failed = True
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 go_to_pose(self, joint_names, positions, time=3.0, wait_for_execution=True): req = ExecuteKnownTrajectoryRequest() req.trajectory.joint_trajectory.joint_names = joint_names p = JointTrajectoryPoint() p.positions = positions p.time_from_start = rospy.Duration(time) req.trajectory.joint_trajectory.points.append(p) req.wait_for_execution = wait_for_execution try: resp = self.ekt_srv.call(req) return resp except rospy.ServiceException as e: rospy.logerr("Service exception: " + str(e))
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 go_to_configuration(self, joint_names, positions, time=None, wait_for_execution=None): """ Execute a trajectory from current configuration to the given configuration. :param list of str joint_names: List of joint names. :param list of float positions: List of positions. """ if len(joint_names) != len(positions): rospy.logerr("joint_names and positions have different lengths!") resp = ExecuteKnownTrajectoryResponse() resp.error_code = 9999 # Failure return resp if time is None: time = self.time if wait_for_execution is None: wait_for_execution = self.wait_for_execution req = ExecuteKnownTrajectoryRequest() req.trajectory.joint_trajectory.joint_names = joint_names p = JointTrajectoryPoint() p.positions = positions p.time_from_start = rospy.Duration(time) req.trajectory.joint_trajectory.points.append(p) req.wait_for_execution = wait_for_execution try: resp = self.ekt_srv.call(req) return resp except rospy.ServiceException as e: rospy.logerr("Service exception: " + str(e)) resp = ExecuteKnownTrajectoryResponse() resp.error_code = 9999 # Failure return resp
def on_enter(self, userdata): self._param_error = False self._request_failed = False self._moveit_failed = False self._success = False # Parameter check if self._srdf_param is None: self._param_error = True return try: self._srdf = ET.fromstring(self._srdf_param) except Exception as e: Logger.logwarn( 'Unable to parse given SRDF parameter: /robot_description_semantic' ) self._param_error = True if not self._param_error: robot = None for r in self._srdf.iter('robot'): if self._robot_name == '' or self._robot_name == r.attrib[ 'name']: robot = r break if robot is None: Logger.logwarn('Did not find robot name in SRDF: %s' % self._robot_name) return 'param_error' config = None for c in robot.iter('group_state'): if (self._move_group == '' or self._move_group == c.attrib['group']) \ and c.attrib['name'] == self._config_name: config = c self._move_group = c.attrib[ 'group'] #Set move group name in case it was not defined break if config is None: Logger.logwarn('Did not find config name in SRDF: %s' % self._config_name) return 'param_error' try: self._joint_config = [ float(j.attrib['value']) for j in config.iter('joint') ] self._joint_names = [ str(j.attrib['name']) for j in config.iter('joint') ] except Exception as e: Logger.logwarn('Unable to parse joint values from SRDF:\n%s' % str(e)) return 'param_error' # Action Initialization action_goal = ExecuteKnownTrajectoryRequest( ) # ExecuteTrajectoryGoal() #action_goal.trajectory.joint_trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.3) action_goal.trajectory.joint_trajectory.joint_names = self._joint_names action_goal.trajectory.joint_trajectory.points = [ JointTrajectoryPoint() ] action_goal.trajectory.joint_trajectory.points[ 0].time_from_start = rospy.Duration(self._duration) action_goal.wait_for_execution = self._wait_for_execution action_goal.trajectory.joint_trajectory.points[ 0].positions = self._joint_config try: self._response = self._client.call(self._action_topic, action_goal) Logger.loginfo( "Execute Known Trajectory Service requested: \n" + str(self._action_topic)) except rospy.ServiceException as exc: Logger.logwarn( "Execute Known Trajectory Service did not process request: \n" + str(exc)) self._request_failed = True return
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 on_enter(self, userdata): self._param_error = False self._request_failed = False self._moveit_failed = False self._success = False # Parameter check if self._srdf_param is None: self._param_error = True return try: self._srdf = ET.fromstring(self._srdf_param) except Exception as e: Logger.logwarn('Unable to parse given SRDF parameter: /robot_description_semantic') self._param_error = True if not self._param_error: robot = None for r in self._srdf.iter('robot'): if self._robot_name == '' or self._robot_name == r.attrib['name']: robot = r break if robot is None: Logger.logwarn('Did not find robot name in SRDF: %s' % self._robot_name) return 'param_error' config = None for c in robot.iter('group_state'): if (self._move_group == '' or self._move_group == c.attrib['group']) \ and c.attrib['name'] == self._config_name: config = c self._move_group = c.attrib['group'] #Set move group name in case it was not defined break if config is None: Logger.logwarn('Did not find config name in SRDF: %s' % self._config_name) return 'param_error' try: self._joint_config = [float(j.attrib['value']) for j in config.iter('joint')] self._joint_names = [str(j.attrib['name']) for j in config.iter('joint')] except Exception as e: Logger.logwarn('Unable to parse joint values from SRDF:\n%s' % str(e)) return 'param_error' # Action Initialization action_goal = ExecuteKnownTrajectoryRequest() # ExecuteTrajectoryGoal() #action_goal.trajectory.joint_trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.3) action_goal.trajectory.joint_trajectory.joint_names = self._joint_names action_goal.trajectory.joint_trajectory.points = [JointTrajectoryPoint()] action_goal.trajectory.joint_trajectory.points[0].time_from_start = rospy.Duration(self._duration) action_goal.wait_for_execution = self._wait_for_execution action_goal.trajectory.joint_trajectory.points[0].positions = self._joint_config try: self._response = self._client.call(self._action_topic, action_goal) Logger.loginfo("Execute Known Trajectory Service requested: \n" + str(self._action_topic)) except rospy.ServiceException as exc: Logger.logwarn("Execute Known Trajectory Service did not process request: \n" + str(exc)) self._request_failed = True return